

clc
clear all
close all

particles = 300;

%% position (x)
figure

noise = 0.04;
lambda = 10;

%highway
subplot(211)
x=[1.09678 1.52605 1.44882 1.46457 1.47868 1.50049 4.76921 1.66179 1.80373 1.49822 -0.198508 1.35665 4.99163 1.43923 1.41372 1.26914 1.20314 1.27798 1.22824 1.22455 1.24178 1.66684 0.892909 0.0750414 1.09877 1.16617 1.25635 1.29363 1.24669 1.19608 1.23981 1.23156 1.2534 1.2597 1.28054 1.29136 1.32348 1.33524 1.3529 1.36595 1.34453 1.32246 1.36302 1.49503 1.31312 1.37534 1.40175 1.36446 1.41192 1.42295 1.43905 1.4862 1.49744 1.52119 1.5349 1.56273 1.54364 1.56478 1.57972 1.60298 1.59659 1.59584 1.66879 1.65748 1.6532 1.72282 1.74551 1.73509 1.74772 1.78021 1.75818 1.76141 1.7579 1.80292 1.78739 1.79822 1.8162 1.8226 1.8612 1.83598 1.86448 1.8572 1.87254 1.85781 1.93619 1.87575 1.93486 1.92546 1.90661 1.92484 1.95427 1.9334 1.99291 2.02947 2.01705 2.04383 2.01423 2.0345 1.92296 2.02302 2.05926 2.11239 2.10563 2.09017 2.05451 2.06726 2.10432 1.99471 1.98407 2.04814 2.00165 2.04998 2.05466 2.07323 2.07061 2.08448 2.052 2.11285 2.10656 2.13056 2.15806 2.18224 2.10323 2.1717 2.18566 2.15391 2.16766 2.14909 2.16821 2.1521 2.12013 2.19482 2.13823 2.20497 2.24642 2.191 2.24084 2.26195 2.24354 2.2503 2.27989 2.22965 2.19963 2.27479 2.27364 2.23138 2.2676 2.23685 2.33398 2.12314 2.12706 2.36819 2.28989 2.33417 2.38267 2.29189 2.32993 2.33192 2.39876 2.36464 2.36242 2.42706 2.42182 2.40027 2.41793 2.2646 2.27841 2.26939 2.35003 2.36817 2.48632 2.45148 2.34446 2.38421 2.39145 2.46784 2.41203 2.37645 2.45151 2.46395 2.40557 2.4564 2.53874 2.45654 2.43877 2.54387 2.59959 2.623 2.65099 2.63398 2.56495 2.53095 2.59905 2.4991 2.53246 2.61465 2.59239 2.55024 2.59328 2.55859 2.51401 2.60964 2.68277 2.43824 3.17659 2.2039 2.75029 1.33708 2.69303 3.20496 2.91009 2.67278 3.06277 2.83398 2.73226 4.04937 3.05141 2.62307 3.41063 -0.0155709 3.05725 10.606 3.94063 2.97681 3.33358 11.239 2.55359 3.40074 3.97094 3.26129 4.68894 3.02875 3.19873 3.1735 2.89044 2.70036 2.55947 3.71885 2.86037 -0.269099 4.77565 2.96066 2.71088 5.32684 2.69514 2.51092 3.27741 4.29026 2.51839 34.2712 3.63978 0.41685 3.2598 3.90064 2.19539 3.04856 2.75057 3.99649 3.46916 3.51984 3.83226 4.25521 2.72567 4.04396 4.63504 3.29844 3.82158 2.68173 6.18787 4.24657 3.54319 3.93162 12.8025 13.8119 5.26879 18.3347 3.88885 2.89065 5.38277 19.8313 4.53603 5.54912 7.67832 6.26973 4.70436 4.91443];
x=x';
x_filtered = pf_filter(x, particles, noise, lambda);
plot(1:length(x),x,'b', 1:length(x),x_filtered,'r');
legend('x', 'x filtered')

%city
subplot(212)
x=[0.777281 0.79611 0.823118 0.823774 0.823415 0.825718 0.829798 0.833647 0.852967 0.86072 0.874046 0.8799 0.886297 0.893481 0.907578 0.909541 0.916217 0.933244 0.933889 0.955032 0.957893 0.95865 0.97213 0.982493 0.998256 1.00732 1.02267 1.03578 1.05185 1.06638 1.07278 1.09269 1.11306 1.11776 1.12395 1.13145 1.12656 1.13428 1.13198 1.12272 1.12699 1.12373 1.12007 1.11792 1.11669 1.11474 1.10888 1.11453 1.11127 1.11641 1.10559 1.10301 1.10911 1.10509 1.10479 1.09832 1.08825 1.08757 1.08552 1.0879 1.07724 1.0717 1.07313 1.07499 1.06259 1.07177 1.06073 1.07194 1.06713 1.05391 1.04111 1.0357 1.04505 1.04725 1.04861 1.05871 1.06246 1.05924 1.07011 1.07215 1.07641 1.07695 1.08352 1.08783 1.08477 1.08319 1.09354 1.09098 1.09798 1.11015 1.12391 1.11399 1.12879 1.12672 1.12421 1.11756 1.117 1.10965 1.12274 1.10023 1.09452 1.08254 1.12751 1.07595 1.08502 1.07764 1.07571 1.02446 1.05956 1.05102 1.04592 1.02609 1.00691 0.993587 0.970897 0.956818 0.943547 0.9388 0.902182 0.886705 0.868656 0.851795 0.844345 0.841392 0.830046 0.80461 0.790186 0.767634 0.729482 0.711212 0.675814 0.646267 0.615242 0.578983 0.548467 0.511053 0.486121 0.457418 0.434223 0.410531 0.397751 0.386392 0.383635 0.37495 0.377375 0.363392 0.354033 0.33446 0.334844 0.334264 0.330721 0.343225 0.343255 0.332377 0.338401 0.359401 0.325636 0.321023 0.322419 0.318828 0.325415 0.321697 0.321134 0.32362 0.32065 0.313997 0.297009 0.316921 0.325083 0.330466 0.347361 0.370298 0.389097 0.408633 0.425294 0.440252 0.429988 0.439789 0.457144 0.468849 0.490771 0.516247 0.532793 0.545862 0.546905 0.556338 0.557918 0.57155 0.585737 0.596183 0.604816 0.608206 0.613682 0.6073 0.612803 0.601732 0.605949 0.591758 0.56949 0.554853 0.529699 0.507974 0.488477 0.478344 0.476742 0.469972 0.436268 0.410397 0.378509 0.361901 0.327855 0.301102 0.276864 0.260203 0.237182 0.216422 0.210381 0.20452 0.205781 0.211642 0.211078 0.208019 0.200428 0.19845 0.195839 0.200358 0.216846 0.240092 0.26818 0.301058 0.324877 0.352024 0.377126 0.403644 0.42617 0.45392 0.475265 0.504537 0.52755 0.550509 0.570927 0.582792 0.603986 0.623152 0.647589 0.672435 0.697443 0.703165 0.721748 0.74512 0.775069 0.777676 0.797124 0.814461 0.824617 0.837422 0.848189 0.866023 0.879019 0.91347 0.924494 0.926355 0.938848 0.949527 0.955247 0.966972 0.989153 0.983912 0.991321 0.999042 1.01754 1.03876 1.02626 1.02833 1.03436 1.03881 1.04261 1.04829 1.03777 1.03267 1.02461 1.0304 1.03244 1.02899 1.02713 1.01435 1.02011 1.00256 0.999002 1.00118 1.00739 1.00306 0.991878 0.999502 0.989989 0.984999 0.983786 0.986576 0.958975 0.963404 0.953301 0.942174 0.915386 0.911487 0.892452 0.878224 0.85129 0.829422 0.817308 0.812849 0.802726 0.790689 0.783687 0.774282 0.766113 0.740768 0.728656 0.709279 0.684199 0.655229 0.647809 0.626851 0.585593 0.55949 0.523177 0.48952 0.471826 0.436654 0.428487 0.427877 0.438875 0.426087 0.416654 0.417495 0.422779 0.406565 0.398754 0.392007 0.390939 0.400421 0.411733 0.428044 0.432652 0.440988 0.45363 0.465093 0.48003 0.494043 0.501006 0.514027 0.517289 0.516887 0.521841 0.52746 0.537642 0.543286 0.545341 0.552278 0.552872 0.553182 0.555188 0.554082 0.564594 0.58108 0.589874 0.584676 0.582241 0.577556 0.573987 0.588299 0.603379 0.627752 0.633199 0.641736 0.652927 0.67419 0.692325 0.709999 0.732258 0.751561 0.778039 0.803932 0.825746 0.844096 0.869098 0.897626 0.91694 0.928683 0.951305 0.958106 0.981539 0.997626 1.01098 1.03157 1.03987 1.06245 1.07623 1.08492 1.09856 1.11242 1.12686 1.13534 1.15764 1.17025 1.19143 1.19618 1.20326 1.21948 1.21612 1.22644 1.21078 1.21675 1.20755 1.22867 1.21745 1.2086 1.20798 1.20758 1.21033 1.20525 1.20558 1.2017 1.20163 1.20542 1.19677 1.19937 1.18563 1.19847 1.19785 1.1923 1.18904 1.18228 1.19681 1.18697 1.20218 1.17131 1.18858 1.19939 1.1707 1.15388 1.15936 1.14483 1.11142 1.08791 1.08652 1.05561 1.0527 1.04563 1.02347 1.01355 1.01044 0.965804 0.958236 0.934571 0.911676 0.847 0.877612 0.860342 0.83911 0.801834 0.810298 0.807918 0.767278 0.746782 0.766063 0.715097 0.68141 0.674402 0.609364 0.593073 0.591573 0.59924 0.576922 0.559496 0.564598 0.544275 0.548262 0.566355 0.548065 0.543979 0.535572 0.519721 0.526976 0.548941 0.543215 0.532901 0.532947 0.550724 0.521159 0.542875 0.539013 0.525202 0.522489 0.541018 0.522494 0.529913 0.531406 0.526246 0.534087 0.524995 0.550949 0.536507 0.536631 0.545668 0.547565 0.552013 0.551745 0.546466 0.538447 0.543638 0.524159 0.515255 0.511013 0.487957 0.466481 0.44645 0.443541 0.432334 0.397823 0.399989 0.366662 0.36021 0.349959 0.341018 0.349997 0.33259 0.351083 0.351884 0.354231 0.357961 0.360731 0.359306 0.358021 0.367232 0.364902 0.360471 0.365898 0.376529 0.384964 0.384565 0.387305 0.401741 0.427197 0.42401 0.442171 0.462594 0.480802 0.492701 0.519882 0.555015 0.578428 0.596203 0.620994 0.640743 0.621599 0.634258 0.658122 0.681403 0.688944 0.716154 0.752011 0.734558 0.802121 0.786054 0.820829 0.840777 0.876531 0.880769 0.908685 0.939836 0.958591 0.973937 0.998161 1.01291 1.01918 1.04941 1.07765 1.09 1.10273 1.10488 1.10555 1.12072 1.13247 1.14623 1.15593 1.15635 1.17763 1.17792 1.19205 1.2003 1.21918 1.21189 1.22277 1.22663 1.22386 1.23359 1.23629 1.24677 1.25543 1.25811 1.26351 1.26967 1.26643 1.26142 1.28272 1.29991 1.30407 1.32389 1.31692 1.35421 1.32821 1.33264 1.34306 1.31806 1.33296 1.34678 1.33754 1.41748 1.35645 1.34056 1.33544 1.36836 1.34557 1.35671 1.34093 1.3513 1.33418 1.33331 1.3443 1.34667 1.33959 1.32798 1.32693 1.32828 1.33069 1.30576 1.32884 1.31088 1.32295 1.32687 1.31118 1.32729 1.32312 1.37142 1.3158 1.3194 1.33277 1.31697 1.34266 1.32891 1.34351 1.34848 1.30499 1.32446 1.33199 1.32568 1.32804 1.34195 1.34697 1.35082 1.35638 1.35616 1.35869 1.36757 1.38891 1.33562 1.37606 1.38024 1.36038 1.369 1.36889 1.38343 1.31312 1.36422 1.38964 1.37753 1.38509 1.37394 1.34067 1.37602 1.34899 1.34639 1.345 1.34865 1.34713 1.35165 1.33302 1.3714 1.37037 1.32832 1.33331 1.32104 1.30737 1.30408 1.30182 1.31282 1.29928 1.28357 1.28015 1.27682 1.26036 1.24832 1.2461 1.23453 1.25138 1.23988 1.21429 1.23843 1.2114 1.19233 1.18375 1.17212 1.12737 1.10651 1.06835 1.04096 1.01231 1.00449 0.96063 0.940805 0.906176 0.882012 0.858535 0.827191 0.806906 0.788664 0.756444 0.730965 0.696139 0.666521 0.627622 0.610505 0.58986 0.572417 0.557444 0.545659 0.530417 0.529192 0.520206 0.525082 0.509527 0.509755 0.498037 0.500547 0.491857 0.47791 0.470123 0.454088 0.441908 0.444281 0.417863 0.401694 0.40116 0.390075 0.385318 0.372246 0.376846 0.386057 0.393899 0.394875 0.386258 0.385726 0.385987 0.378587 0.395267 0.385468 0.385785 0.380489 0.391149 0.375863 0.39616 0.395133 0.41996 0.452861 0.434664 0.434357 0.47737 0.458701 0.484889 0.446518 0.424578 0.423731 0.437059 0.458441 0.463687 0.501681 0.504982 0.508652 0.501332 0.512209 0.524204 0.534433 0.55308 0.573566 0.587931 0.604452 0.615139 0.634408 0.655901 0.666209 0.683001 0.70198 0.729122 0.723878 0.74441 0.759071 0.775342 0.786515 0.808661 0.836473 0.830478 0.825744 0.839975 0.843198 0.849374 0.840936 0.827513 0.812777 0.822723 0.813227 0.820178 0.813287 0.803805 0.795628 0.770985 0.765716 0.745565 0.73526 0.722361 0.674844 0.65422 0.636822 0.657814 0.648326 0.654949 0.632475 0.625934 0.616425 0.610286 0.608923 0.603823 0.595383 0.592736 0.589692 0.567758 0.58181 0.577151 0.559887 0.567658 0.532522 0.532574 0.535589 0.56538 0.540287 0.531996 0.496239 0.484758 0.502281 0.490879 0.48661 0.487038 0.492752 0.491331 0.491935 0.51399 0.515065 0.521397 0.540021 0.55046 0.536675 0.567139 0.575221 0.58592 0.590553 0.611042 0.627952 0.64629 0.66767 0.671979 0.666187 0.675894 0.6884 0.710959 0.718409 0.732847 0.74258 0.753956 0.74963 0.761771 0.773868 0.789781 0.81813 0.816794 0.820696 0.826383 0.836265 0.847671 0.855565 0.837436 0.86066 0.853489 0.853154 0.864084 0.874816 0.872319 0.866902 0.864661 0.864199 0.879873 0.880503 0.888318 0.885453 0.854438 0.869468 0.875395 0.867997 0.88286 0.896374 0.905103 0.923378 0.931759 0.951726 0.958745 0.971606 1.01632 1.03579 1.03153 1.03946 1.06267 1.09658 1.10114 1.11299 1.1367 1.13184 1.14929 1.17389 1.20375 1.20819 1.22372 1.22867 1.21568 1.25982 1.27571 1.2787 1.27333 1.27251 1.27205 1.2663 1.26742 1.26912 1.27501 1.25555 1.2746 1.26042 1.26542 1.26055 1.25221 1.2483 1.25208 1.23528 1.23379 1.24123 1.24064 1.22667 1.23022 1.22826 1.23033 1.20398 1.22019 1.24039 1.22652 1.21947 1.20425 1.22209 1.21719 1.21629 1.21819 1.18644 1.18054 1.16412 1.17145 1.16562 1.15042 1.151 1.14019 1.11975 1.12552 1.13719 1.1517 1.14767 1.16602 1.1572 1.12859 1.12295 1.12698 1.13471 1.14568 1.1476 1.13959 1.14306 1.14271 1.1503 1.14615 1.14473 1.14818 1.14972 1.15708 1.16528 1.17354 1.17176 1.1725 1.16804 1.17363 1.18524 1.20124 1.21355 1.2171 1.22478 1.22098 1.21747 1.23386 1.23299 1.23968 1.25449 1.25215 1.25981 1.26823 1.27258 1.28419 1.28766 1.29103 1.2933 1.29368 1.29287 1.30084 1.29286 1.29843 1.30223 1.30416 1.31365 1.30765 1.31121 1.31362 1.31126 1.31686 1.31332 1.31439 1.33154 1.32792 1.32261 1.32125 1.31952 1.3171 1.30625 1.30214 1.31156 1.30824 1.32324 1.30325 1.306 1.30367 1.32551 1.31291 1.30931 1.32148 1.31292 1.3023 1.31828 1.30473 1.31776 1.31406 1.30759 1.28312 1.30296 1.28942 1.27779 1.30003 1.29475 1.28599 1.28645 1.28541 1.26466 1.26207 1.2474 1.2411 1.21992 1.2066 1.19764 1.18399 1.18124 1.16196 1.15046 1.14076 1.12168 1.10495 1.08706 1.06944 1.05489 1.03967 1.03074 1.01725 1.00198 0.977212 0.962468 0.946353 0.930434 0.918274 0.893756 0.884751 0.854591 0.828456 0.803004 0.786913 0.75659 0.749411 0.720939 0.695007 0.665768 0.648886 0.621415 0.605621 0.585553 0.563256 0.549991 0.544159 0.51683 0.510515 0.512784 0.507339 0.507007 0.502913 0.498196 0.488777 0.497487 0.486144 0.480707 0.469852 0.456846 0.44095 0.447345 0.447268 0.446386 0.446138 0.434483 0.433997 0.387328 0.430303 0.437778 0.452628 0.458195 0.473194 0.482981 0.503991 0.527583 0.556597 0.570018 0.596942 0.614093 0.629929 0.657799 0.67176 0.699 0.723043 0.751432 0.775581 0.801076 0.816087 0.837101 0.845424 0.880602 0.905906 0.92006 0.933201 0.960658 0.958168 0.969935 0.985529 0.991489 1.00614 1.02391 1.03553 1.0378 1.03702 1.02271 1.04125 1.05482 1.05249 1.05119 1.04671 1.04076 1.06656 1.0616 1.07222 1.06815 1.05557 1.06331 1.07921 1.08623 1.0791 1.07789 1.08119 1.07075 1.06295 1.06833 1.07623 1.07158 1.07037 1.0593 1.06409 1.04298 1.03366 1.04686 1.05722 1.02742 1.04655 1.02386 1.0314 1.05035 1.04001 1.04096 1.03992 1.00845 1.01299 1.01486 1.01189 1.00608 1.0204 1.02432 1.01322 1.00991 1.00394 1.00432 0.983557 1.00069 0.989503 1.00755 1.01833 1.005 1.02534 1.00792 0.995145 0.99245 0.991683 0.999071 1.01648 1.00846 1.00749 1.0267 1.0625 0.994641 0.985818 0.977839 0.979132 0.98058 0.969239 0.967455 0.964586 0.974733 0.967452 0.959077 0.946954 0.942962 0.942501 0.932046 0.942634 0.935296 0.929246 0.925642 0.928816 0.92376 0.931682 0.945571 0.933309 0.933423 0.946288 0.982622 0.96618 0.97199 0.976268 0.962312 0.991645 1.00742 1.01954 1.03919 1.02965 1.03932 1.05667 1.06091 1.06852 1.08438 1.08439 1.10686 1.13338 1.12973 1.1581 1.13991 1.1506 1.14217 1.12758 1.14285 1.15034 1.15858 1.15771 1.17409 1.19114 1.17663 1.178 1.17106 1.16501 1.16456 1.16368 1.16945 1.13709 1.17533 1.15851 1.16211 1.14675 1.15657 1.15941 1.14898 1.15297 1.1461 1.13717 1.13324 1.14353 1.15709 1.11674 1.10858 1.11457 1.10361 1.09542 1.10502 1.1224 1.10727 1.06845 1.0705 1.05995 1.04811 1.06336 1.02863 1.05595 1.01093 0.997234 0.988992 1.01428 0.987473 0.97386 0.971403 0.964366 0.970042 0.956401 0.960014 0.97518 0.951717 0.917367 0.902794 0.934661 0.908593 0.884724 0.867496 0.85804 0.839857 0.817191 0.830253 0.800584 0.780663 0.756335 0.734932 0.718158 0.70175 0.690088 0.664446 0.640636 0.626675 0.599435 0.57427 0.546109 0.516565 0.488534 0.456745 0.452066 0.426371 0.394874 0.377444 0.350095 0.339867 0.322404 0.322173 0.324321 0.338583 0.342027 0.340891 0.329573 0.331298 0.350569 0.36878 0.365077 0.386719 0.396136 0.42362 0.461528 0.459487 0.485501 0.49861 0.520446 0.538172 0.546591 0.592842 0.591954 0.615308 0.63164 0.630131 0.642964 0.654802 0.664749 0.68045 0.685852 0.662571 0.655605 0.696686 0.660495 0.667517 0.669379 0.667594 0.676204 0.670224 0.658119 0.656438 0.639417 0.642903 0.665899 0.636995 0.632473 0.592636 0.575384 0.569389 0.567584 0.573588 0.574719 0.581485 0.555297 0.550509 0.539891 0.540323 0.567873 0.584698 0.572734 0.568626 0.572806 0.544428 0.54318 0.568125 0.568495 0.572775 0.596524 0.559882 0.558178 0.588638 0.562796 0.585685 0.586977 0.582694 0.604082 0.570111 0.572776 0.559632 0.568375 0.573928 0.589808 0.583426 0.600549 0.620539 0.634202 0.649311 0.669092 0.685948 0.695554 0.712098 0.732621 0.753894 0.777884 0.799151 0.80259 0.833194 0.859139 0.888492 0.912727 0.932549 0.933108 0.950379 0.984286 1.01544 1.04592 1.0371 1.0747 1.03208 1.08612 1.08781 1.12492 1.16319 1.15925 1.17123 1.2348 1.22101 1.23863 1.19925 1.23833 1.26243 1.26146 1.27948 1.25582 1.2768 1.26801 1.22991 1.26859 1.27856 1.28224 1.25391 1.25599 1.25866 1.24778 1.23717 1.27468 1.28134 1.26251 1.26118 1.29134 1.28396 1.27588 1.2499 1.27912 1.25327 1.26267 1.26525 1.24955 1.25171 1.25141 1.2772 1.27169 1.30831 1.29956 1.28819 1.26446 1.2703 1.28909 1.29154 1.34131 1.30011 1.31265 1.30933 1.32133 1.33743 1.31813 1.35658 1.3328 1.37117 1.31876 1.36861 1.3598 1.33081 1.3474 1.36982 1.34255 1.31401 1.31638 1.32305 1.32575 1.32722 1.30022 1.2714 1.2498 1.24211 1.24248 1.22442 1.19428 1.16164 1.13545 1.13431 1.08184 1.07395 1.04343 1.03429 1.0096 0.993012 0.958478 0.921994 0.900668 0.885935 0.874815 0.821655 0.794147 0.779314 0.77063 0.73154 0.741533 0.737871 0.729714 0.740576 0.751364 0.823611 0.728954 0.739002 0.74063 0.740009 0.718667 0.712797 0.735931 0.731221 0.745353 0.734201 0.722855 0.727559 0.71925 0.729888 0.71357 0.70217 0.711671 0.704809 0.69237 0.683416 0.694373 0.701891 0.714712 0.654973 0.695898 0.706909 0.683135 0.682476 0.67686 0.691956 0.683096 0.674555 0.660573 0.67033 0.67442 0.657618 0.654301 0.659468 0.646939 0.673464 0.670814 0.672151 0.646312 0.657935 0.676166 0.688273 0.711897 0.731821 0.747387 0.762582 0.780156 0.799449 0.83452 0.836433 0.853769 0.87319 0.893701 0.908647 0.939142 0.949983 0.97227 0.989321 1.0013 1.01958 1.03191 1.03938 1.05157 1.06166 1.06954 1.0751 1.0907 1.09502 1.09356 1.12624 1.12521 1.13201 1.13397 1.13814 1.15017 1.17103 1.16405 1.216 1.1814 1.19594 1.18397 1.20401 1.20768 1.21033 1.22153 1.24165 1.24578 1.24873 1.26595 1.23494 1.24252 1.25876 1.22393 1.22562 1.21649 1.22423 1.21951 1.21474 1.2227 1.22043 1.2354 1.22288 1.23098 1.23967 1.23656 1.23325 1.23956 1.2318 1.23596 1.24031 1.25685 1.24498 1.24754 1.24137 1.24622 1.25177 1.24422 1.23624 1.22856 1.23646 1.23872 1.22671 1.22972 1.24121 1.22195 1.23036 1.23647 1.22244 1.22227 1.23457 1.2667 1.23478 1.21416 1.2248 1.23115 1.21508 1.20832 1.2016 1.21723 1.21191 1.22099 1.22081 1.22424 1.23958 1.23242 1.22243 1.23538 1.25844 1.25469 1.24831 1.22919 1.27949 1.27661 1.29563 1.27932 1.27053 1.26384 1.2324 1.23464 1.27501 1.29606 1.26625 1.25518 1.26906 1.21856 1.2645 1.26315 1.22768 1.27788 1.23318 1.24468 1.25458 1.21063 1.25924 1.24513 1.23406 1.24175 1.25406 1.22251 1.22541 1.22087 1.24446 1.21831 1.23511 1.21277 1.21551 1.19124 1.19448 1.19026 1.1643 1.1819 1.16959 1.16172 1.1782 1.15486 1.1582 1.1477 1.13966 1.12238 1.11186 1.08056 1.05637 1.03423 1.00881 0.982432 0.952758 0.946184 0.919813 0.906913 0.876057 0.859441 0.847377 0.817498 0.797598 0.785524 0.757918 0.73002 0.712005 0.681511 0.666376 0.650526 0.626929 0.613539 0.597271 0.579517 0.570877 0.555149 0.543264 0.53153 0.527517 0.50897 0.517007 0.489004 0.489555 0.495883 0.48899 0.450046 0.454151 0.455431 0.430976 0.417338 0.412764 0.40858 0.408544 0.392866 0.381796 0.38562 0.38776 0.389661 0.376805 0.383939 0.362403 0.366906 0.367228 0.389804 0.370443 0.372222 0.368572 0.364785 0.363612 0.364386 0.362412 0.386705 0.390015 0.378785 0.39312 0.394674 0.428623 0.412477 0.403891 0.391048 0.367887 0.365333 0.381883 0.378368 0.3995 0.415243 0.4225 0.431727 0.433025 0.438513 0.446576 0.47019 0.487305 0.509001 0.529572 0.552023 0.566135 0.590885 0.604655 0.625755 0.645511 0.660991 0.675478 0.69045 0.710473 0.733597 0.754992 0.764216 0.776665 0.781455 0.789122 0.798704 0.804153 0.793307 0.782465 0.811321 0.794231 0.80784 0.842917 0.791174 0.786573 0.790659 0.785511 0.765604 0.76912 0.7654 0.731971 0.732791 0.706986 0.703987 0.683102 0.67824 0.639952 0.613293 0.607259 0.604301 0.594634 0.588182 0.562872 0.548121 0.5326 0.533988 0.505525 0.487039 0.471628 0.456099 0.438432 0.430323 0.420502 0.416269 0.408454 0.393974 0.38758 0.37725 0.3728 0.369083 0.375953 0.386422 0.391318 0.400695 0.411938 0.420491 0.412479 0.432962 0.436273 0.446325 0.44866 0.466161 0.47481 0.488456 0.507888 0.503357 0.505232 0.526085 0.484263 0.505795 0.478226 0.457029 0.439739 0.455093 0.475621 0.490256 0.495378 0.515131 0.526483 0.529087 0.543362 0.550356 0.554232 0.559081 0.564333 0.575023 0.575451 0.577213 0.575061 0.569978 0.574844 0.591441 0.608836 0.592585 0.58411 0.556063 0.548717 0.5627 0.591179 0.592145 0.596842 0.585718 0.585354 0.590515 0.604082 0.621645 0.63485 0.646675 0.658753 0.673411 0.690731 0.704984 0.714838 0.748858 0.761416 0.778716 0.80145 0.817982 0.833851 0.851713 0.873756 0.890914 0.910848 0.928039 0.950989 0.960783 0.994471 1.00135 1.01445 1.02505 1.03062 1.03147 1.03339 1.03595 1.03228 1.02585 1.02956 1.02957 1.03853 1.03416 1.03366 1.0307 1.03231 1.03306 1.03717 1.03318 1.03337 1.02791 1.02964 1.0289 1.01976 1.02387 1.02624 1.03011 1.02493 1.0302 1.02179 1.02183 1.01288 1.01781 1.01368 1.01363 1.00301 1.00764 1.00417 0.999533 0.987277 0.982271 0.966713 0.963502 0.938149 0.937523 0.926369 0.912781 0.895162 0.901501 0.883116 0.869034 0.852597 0.852678 0.837926 0.814042 0.806588 0.789607 0.783754 0.77128 0.759422 0.744152 0.72347 0.701128 0.696822 0.683432 0.651072 0.656367 0.647963 0.617653 0.626518 0.612401 0.600242 0.591595 0.584611 0.565832 0.570864 0.550666 0.545495 0.545336 0.534405 0.527952 0.519172 0.516463 0.520989 0.512201 0.505946 0.501126 0.512354 0.507924 0.512846 0.507334 0.50062 0.527815 0.531416 0.508603 0.494325 0.472813 0.477143 0.501115 0.488571 0.481255 0.480897 0.491585 0.495378 0.500077 0.50217 0.506275 0.502435 0.521127 0.524999 0.526085 0.53826 0.530723 0.543333 0.538757 0.549415 0.557786 0.571062 0.584352 0.612345 0.59731 0.582197 0.579341 0.597321 0.619003 0.663977 0.665581 0.66493 0.675549 0.684398 0.699604 0.719436 0.733326 0.749268 0.767938 0.777386 0.792754 0.813896 0.831691 0.837502 0.848843 0.844952 0.847315 0.852349 0.852313 0.853125 0.84579 0.839992 0.839999 0.835953 0.832676 0.831249 0.83094 0.829997 0.824653 0.823111 0.814288 0.810657 0.804553 0.807923 0.811039 0.80632 0.805319 0.806284 0.800371 0.796393 0.791932 0.795125 0.800125 0.80038 0.804821 0.802137 0.800726 0.797627 0.794504 0.797585 0.778214 0.772784 0.756243 0.742151 0.720519 0.706045 0.68781 0.668133 0.651408 0.648756 0.639668 0.640574 0.644172 0.63621 0.635466 0.63223 0.628451 0.633547 0.635761 0.648166 0.654092 0.66898 0.681662 0.69618 0.706461 0.722693 0.738948 0.754836 0.765246 0.780735 0.795659 0.807745 0.815488 0.822882 0.828194 0.838844 0.84037 0.845923 0.861681 0.867775 0.890844 0.898456 0.91401 0.9248 0.933411 0.948443 0.966823 0.983442 0.997848 1.01156 1.04237 1.04903 1.071 1.08729 1.10487 1.11439 1.13019 1.13183 1.13739 1.12524 1.14202 1.13148 1.12965 1.12402 1.12864 1.12381 1.12343 1.12477 1.12476 1.10949 1.11937 1.109 1.09575 1.07108 1.05002 1.02187 1.00881 0.993401 0.987519 0.966536 0.95902 0.943217 0.927035 0.917581 0.900202 0.879949 0.862949 0.853921 0.848161 0.838943 0.83711 0.811361 0.799333 0.797493 0.782781 0.780131 0.774651 0.75669 0.743198 0.733458 0.722026 0.70339 0.691535 0.683528 0.671575 0.659496 0.640618 0.621729 0.598651 0.57956 0.558194 0.535226 0.514893 0.485457 0.467162 0.448246 0.431186 0.420417 0.414966 0.40267 0.387563 0.382325 0.387487 0.386087 0.381546 0.375651 0.378422 0.373513 0.383407 0.375018 0.379813 0.391235 0.394247 0.404322 0.41624 0.427638 0.431526 0.427893 0.432035 0.444877 0.457586 0.455611 0.467594 0.465371 0.48487 0.499412 0.492978 0.509617 0.521961 0.504484 0.452599 0.456783 0.458722 0.45693 0.461536 0.459965 0.462241 0.469443 0.47234 0.47514 0.475045 0.47412 0.478787 0.485704 0.491881 0.500774 0.49659 0.493553 0.488998 0.487318 0.489756 0.493852 0.503765 0.519091 0.535149 0.549667 0.572519 0.599852 0.623365 0.649867 0.675312 0.696395 0.723803 0.750761 0.776029 0.801065 0.823189 0.847093 0.876367 0.896025 0.92639 0.94584 0.951156 0.974752 0.988819 0.994338 1.01724 1.03694 1.05462 1.05777 1.0714 1.09405 1.10262 1.12586 1.12347 1.13244 1.12701 1.13769 1.14451 1.15015 1.13921 1.14084 1.13664 1.15725 1.15243 1.14947 1.1491 1.13933 1.14459 1.1367 1.13862 1.12377 1.12847 1.12334 1.12423 1.12741 1.11609 1.12809 1.13084 1.13281 1.11952 1.11721 1.13095 1.11286 1.1163 1.10994 1.10741 1.09719 1.10239 1.10134 1.09683 1.08419 1.08419 1.08505 1.07757 1.07842 1.08151 1.07724 1.06432 1.06653 1.07969 1.05812 1.06038 1.06288 1.05975 1.04595 1.0466 1.0315 1.01848 1.01358 1.07375 1.00348 1.00525 0.977818 0.997162 0.979177 0.956418 0.929937 0.94066 0.890811 0.91441 0.862624 0.851015 0.839883 0.81118 0.778145 0.760659 0.746648 0.720693 0.696874 0.664029 0.651246 0.620168 0.591808 0.580724 0.54794 0.531384 0.516453 0.507858 0.491248 0.463362 0.462242 0.468574 0.492418 0.542363 0.498442 0.492489 0.461845 0.447695 0.441483 0.432764 0.431404 0.432644 0.433028 0.439047 0.444488 0.447877 0.462695 0.457877 0.479489 0.480886 0.465111 0.466935 0.483432 0.482538 0.495451 0.494196 0.508146 0.514149 0.518979 0.537255 0.548613 0.561316 0.577582 0.606687 0.619899 0.638878 0.652421 0.677152 0.701799 0.722273 0.745227 0.767274 0.78943 0.810103 0.828076 0.852153 0.882953 0.90389 0.921625 0.945304 0.955189 0.988241 1.00053 1.00816 1.04005 1.0411 1.04503 1.05483 1.06517 1.07698 1.08988 1.09471 1.09006 1.10524 1.11577 1.11904 1.12631 1.15218 1.1556 1.15634 1.17667 1.1564 1.15826 1.18397 1.18252 1.19235 1.19361 1.18523 1.18849 1.16744 1.17104 1.17588 1.18003 1.17327 1.15715 1.15949 1.14765 1.16525 1.15573 1.15228 1.14289 1.14097 1.15918 1.1705 1.16015 1.14986 1.14343 1.16007 1.15409 1.15093 1.13585 1.16799 1.17633 1.14693 1.13507 1.12716 1.15476 1.14567 1.14605 1.12031 1.12846 1.12541 1.12604 1.13752 1.11676 1.11139 1.06295 1.08944 1.10623 1.06049 1.06938 1.05708 1.06777 1.0188 1.04976 1.03743 0.998673 1.00949 0.978524 0.995322 0.9414 0.943556 0.928782 0.908339 0.88633 0.869299 0.862436 0.852034 0.845765 0.831346 0.78612 0.801854 0.781631 0.760646 0.722386 0.698497 0.689033 0.660283 0.648297 0.618266 0.622453 0.605404 0.583738 0.577927 0.547104 0.527056 0.553178 0.505585 0.489564 0.472376 0.452436 0.438937 0.421189 0.397664 0.385852 0.410835 0.38119 0.370765 0.374193 0.364479 0.342241 0.338379 0.351934 0.343467 0.349016 0.361683 0.367759 0.383346 0.375531 0.404488 0.397396 0.404818 0.408091 0.419442 0.430249 0.43796 0.463145 0.480942 0.488002 0.542715 0.562587 0.597758 0.582261 0.608468 0.601458 0.652169 0.669084 0.679598 0.698985 0.709647 0.755175 0.742449 0.750667 0.761463 0.777039 0.773048 0.797015 0.812831 0.853627 0.877038 0.85837 0.872112 0.87352 0.906249 0.915569 0.91715 0.931347 0.956254 0.980928 0.950594 1.00201 0.984567 1.00922 1.01892 1.03121 1.04125 1.03453 1.05869 1.06647 1.07883 1.09905 1.12528 1.12356 1.13194 1.15224 1.14686 1.17414 1.20258 1.198 1.19562 1.20681 1.22266 1.24603 1.2529 1.24854 1.27579 1.29256 1.32737 1.30115 1.319 1.32376 1.32114 1.34256 1.34388 1.32658 1.31697 1.31974 1.31954 1.28592 1.31362 1.29196 1.27362 1.31355 1.3213 1.3093 1.27702 1.30389 1.30711 1.28321 1.25771 1.27663 1.2887 1.29254 1.28576 1.25279 1.27583 1.2748 1.27557 1.29564 1.28111 1.27096 1.23148 1.24972 1.25458 1.25345 1.24815 1.21477 1.16819 1.14743 1.13595 1.1048 1.08278 1.09003 1.03819 1.0189 1.01216 0.990454 0.97695 0.952337 0.988204 0.954625 0.957332 0.954741 0.968254 0.934323 0.924764 0.924122 0.920154 0.934354 0.950733 0.95671 0.952044 0.970182 0.97777 0.991703 1.01899 1.01066 1.01504 1.03001 1.01851 1.06047 1.05423 1.06052 1.04203 1.05848 1.06523 1.06369 1.09596 1.09245 1.11806 1.10347 1.1056 1.11581 1.11166 1.11257 1.10822 1.10479 1.08497 1.08701 1.10152 1.08369 1.08368 1.07519 1.0598 1.15586 1.02221 1.05056 1.01037 0.982431 0.967257 0.965052 0.950551 0.92301 0.914686 0.905043 0.889712 0.878049 0.878029 0.855739 0.838769 0.822399 0.812119 0.7781 0.75943 0.735226 0.709654 0.690889 0.675013 0.646052 0.610297 0.579063 0.548186 0.528988 0.50798 0.485987 0.467761 0.456098 0.458338 0.452942 0.454204 0.442237 0.436127 0.429865 0.426733 0.421242 0.426078 0.42753 0.43223 0.447542 0.460986 0.462766 0.480329 0.491649 0.491892 0.501537 0.522807 0.527737 0.532592 0.543548 0.54748 0.549783 0.567165 0.580766 0.586776 0.612751 0.631828 0.654042 0.673066 0.689616 0.709988 0.719921 0.732119 0.753847 0.768726 0.790458 0.806502 0.824037 0.837799 0.853315 0.863434 0.87382 0.888606 0.897658 0.913639 0.939523 0.944337 0.966706 0.978754 0.996478 1.01511 1.00882 1.03432 1.04372 1.04836 1.05667 1.0844 1.08892 1.1075 1.11975 1.13321 1.13474 1.14541 1.13984 1.1376 1.16476 1.17678 1.18164 1.18366 1.18555 1.20586 1.20518 1.19361 1.2177 1.21416 1.2289 1.24893 1.24251 1.25681 1.26041 1.26704 1.26598 1.27009 1.28231 1.24964 1.25111 1.24435 1.24494 1.22741 1.23848 1.2323 1.24665 1.26621 1.23702 1.25462 1.22923 1.20781 1.19687 1.21979 1.20635 1.19088 1.19818 1.18531 1.17648 1.18164 1.1885 1.16875 1.16142 1.14808 1.14615 1.14854 1.15118 1.14099 1.12567 1.13907 1.12309 1.12218 1.12821 1.10942 1.09631 1.07784 1.06198 1.04281 1.035 1.00397 0.990347 0.972858 0.967655 0.936823 0.927084 0.909927 0.898435 0.878556 0.856183 0.846129 0.831206 0.824235 0.810948 0.808225 0.805391 0.807324 0.804556 0.788018 0.773337 0.774035 0.787439 0.781701 0.787051 0.79069 0.79926 0.811408 0.826335 0.841836 0.857906 0.869442 0.871473 0.887977 0.891355 0.911464 0.92758 0.936554 0.945276 0.958799 0.962667 0.981701 0.994299 1.00853 1.01197 1.01871 1.02594 1.03132 1.03544 1.05403 1.0649 1.07455 1.07233 1.06958 1.09244 1.09982 1.09286 1.10657 1.11275 1.14178 1.14445 1.14453 1.14763 1.16706 1.171 1.18472 1.18898 1.20485 1.20688 1.21721 1.2344 1.23789 1.24771 1.26126 1.27446 1.28726 1.2881 1.29581 1.31725 1.32838 1.33201 1.33946 1.35662 1.35243 1.35006 1.33835 1.35268 1.36084 1.36247 1.34737 1.34808 1.33224 1.32903 1.32941 1.31023 1.31306 1.31659 1.30992 1.32968 1.30274 1.29551 1.2959 1.29685 1.30036 1.3187 1.27006 1.26338 1.25095 1.22049 1.21825 1.19979 1.17837 1.15884 1.16574 1.16275 1.10921 1.08142 1.07734 1.06991 1.05458 1.04325 1.02298 1.01031 0.981001 0.970468 0.941401 0.942819 0.922481 0.903331 0.899163 0.878351 0.850014 0.850366 0.831141 0.802672 0.776288 0.753101 0.728991 0.711835 0.690974 0.671411 0.660388 0.642486 0.633115 0.619296 0.624766 0.607089 0.599963 0.598041 0.584331 0.579362 0.579494 0.572453 0.572983 0.561549 0.542729 0.550341 0.561295 0.546162 0.552981 0.539923 0.553127 0.539885 0.574123 0.557722 0.573953 0.557024 0.566331 0.546516 0.552278 0.547411 0.550467 0.571697 0.572187 0.582099 0.595145 0.61084 0.611634 0.636174 0.653475 0.664554 0.679072 0.676342 0.697094 0.718229 0.733941 0.760449 0.772704 0.759295 0.73906 0.751839 0.776 0.805558 0.819838 0.826777 0.827737 0.835206 0.852579 0.860568 0.866267 0.876063 0.88183 0.898545 0.911238 0.915803 0.929111 0.927713 0.937904 0.944152 0.945156 0.942939 0.950742 0.953117 0.955468 0.951523 0.952694 0.952679 0.950128 0.944593 0.941025 0.937616 0.942039 0.952173 0.949716 0.954822 0.955032 0.955931 0.960779 0.962207 0.972432 0.980067 0.978152 0.987357 0.993768 1.00036 1.00849 1.00606 1.00612 1.01314 1.01687 1.02422 1.0305 1.04874 1.05541 1.07034 1.07516 1.10044 1.10419 1.11853 1.12434 1.12357 1.12115 1.12336 1.1211 1.11955 1.12078 1.11349 1.11706 1.12165 1.11804 1.11441 1.11854 1.10174 1.10202 1.104 1.08408 1.0802 1.07077 1.05609 1.04964 1.04129 1.04221 1.0219 0.998267 0.987254 0.970689 0.946442 0.930415 0.909694 0.881393 0.872827 0.835314 0.818453];
x_filtered = pf_filter(x, particles, noise, lambda);
plot(1:length(x),x,'b', 1:length(x),x_filtered,'r');
legend('x', 'x filtered')



%% yaw
figure

noise = 0.005; %0.002 / 400
lambda = 250; 

%highway
subplot(211)
yaw=[-2.46554 -2.50393 -2.56313 -2.61218 -2.64623 -2.65858 -2.6556 -2.61076 -2.6091 -2.65705 -2.68904 -2.69451 -2.7613 -2.75847 -2.68735 -2.67682 -2.67782 -2.69624 -2.72474 -2.7014 -2.68408 -2.63055 -2.56773 -2.5207 -2.3832 -2.28343 -2.15603 -2.10071 -2.01547 -1.95943 -1.89388 -1.78893 -1.72582 -1.72255 -1.703 -1.59266 -1.49822 -1.39945 -1.23334 -1.08567 -0.935073 -0.818664 -0.715404 -0.646067 -0.608102 -0.588427 -0.455545 -0.394905 -0.327986 -0.261737 -0.201224 -0.146199 -0.126457 -0.11553 -0.0816895 -0.105996 -0.092285 -0.0132616 0.00496966 0.065726 0.119289 0.102427 0.0726806 0.0768408 0.0497246 0.0424497 0.0890046 0.0581092 0.0841842 0.09157 0.0770311 0.0196075 0.031165 0.011221 0.00682205 0.0292305 -0.0083439 -0.0274765 -0.0404214 -0.0178922 -0.0494006 -0.0222382 -0.0395374 -0.115456 -0.129772 -0.240938 -0.310233 -0.287873 -0.254872 -0.26955 -0.268066 -0.333024 -0.308531 -0.313741 -0.2842 -0.286611 -0.322126 -0.348027 -0.364776 -0.32249 -0.333624 -0.330749 -0.285873 -0.157936 -0.091784 -0.127204 -0.184869 -0.180276 -0.175996 -0.211711 -0.259573 -0.264641 -0.261047 -0.203881 -0.153834 -0.213479 -0.247464 -0.217322 -0.197639 -0.225451 -0.220608 -0.228413 -0.220149 -0.207198 -0.196801 -0.185534 -0.17862 -0.212987 -0.194631 -0.217719 -0.186923 -0.190696 -0.212269 -0.172776 -0.115667 -0.2015 -0.219377 -0.219042 -0.230868 -0.197274 -0.192508 -0.125424 -0.105559 -0.125244 -0.103087 -0.131967 -0.206743 -0.196565 -0.164529 -0.126657 -0.214469 -0.223393 -0.195848 -0.176366 -0.204701 -0.168718 -0.22197 -0.23124 -0.196469 -0.160258 -0.220275 -0.221712 -0.22685 -0.209454 -0.208186 -0.180304 -0.141278 -0.186422 -0.201413 -0.206323 -0.18845 -0.21437 -0.172865 -0.216497 -0.186804 -0.175935 -0.168866 -0.168121 -0.140344 -0.189509 -0.141631 -0.160234 -0.0752495 -0.116639 -0.21414 -0.296555 -0.264389 -0.286055 -0.294821 -0.308136 -0.352093 -0.354072 -0.29619 -0.318841 -0.32235 -0.337364 -0.359274 -0.295219 -0.25376 -0.295116 -0.268503 -0.217291 -0.167904 -0.19848 -0.274758 -0.222536 -0.201361 -0.216377 -0.135314 -0.180025 -0.186983 -0.187723 -0.177622 -0.180194 -0.191615 -0.166362 -0.0773163 -0.0674242 -0.107984 -0.144407 -0.150062 -0.146244 -0.143216 -0.155168 -0.141962 -0.179119 -0.191352 -0.148438 -0.145064 -0.0872197 -0.104759 -0.0790582 -0.0423462 -0.0415956 -0.0622592 -0.065453 -0.0764592 -0.06793 -0.0271102 -0.0334586 -0.0372054 -0.030564 -0.00770894 0.0531046 0.0716656 0.0944479 0.0946059 0.131989 0.192921 0.252715 0.282845 0.243654 0.168983 0.127101 0.0885037 0.0314591 0.0759315 0.0806452 0.103535 0.0713116 0.0832239 0.121246 0.113032 0.0985813 0.113909 0.0920191 0.0772609 0.0541344 0.0861063 0.0369549 0.103382 0.0107681 0.0317063 0.095176 0.0579636 0.0224193 0.0550756 0.0617661 0.0362187 0.0147654 0.0274853 0.0229576 0.0630869 0.0287068 0.0191831 0.0212987 0.0162338 0.0502361 0.0475856 0.0118706 0.0547782 0.0320345 0.0242292 0.00546417 0.00800137 -0.01104 -0.0050621 0.000343506 0.0196509 0.0307702 0.0448038 0.0335784 0.0623744 0.11332 0.0652381 0.038576 0.0417377 0.0252785 0.0141308 0.0191586 0.0507834 0.030333 0.0393737 0.0297524 0.0362637 0.0577521 0.0889028 0.0241181 0.035017 0.0343763 0.0619929 -0.00566863 0.0322658 0.0568188 0.0382368 0.0181184 0.00122374 -0.0181974 -0.00900747 -0.00331155 0.0325289 0.0515911 0.017962 0.00242233 0.00608697 0.0428934 0.0273858 0.0190525 0.0290622 0.100565 0.0645237 0.0801235 0.0895248 0.0463983 0.0208923 0.0270941 0.0647456 0.0541873 0.0369258 0.0780672 0.0338306 0.0330517 0.020455 0.00298376 0.0329056 0.0133136 -0.000286665 -0.0051065 -0.0228829 -0.00925497 0.00655313 0.0465978 0.00659679 0.0164014 -0.00423122 0.0408567 -0.0142999 -0.00121513 0.0229211 0.0235609 0.120543 0.0622883 0.0636732 0.0349305 0.0235503 0.130678 0.0628956 0.0841147 0.0923711 0.0770362 0.0800336 0.0452987 0.0711293 0.101417 0.0924278 0.111702 0.119589 0.0857069 0.117038 0.0722259 0.048582 0.0275482 -0.0163503 0.051595 0.0624597 0.0976467 0.0694796 0.093102 0.0825654 0.053206 0.0770641 0.114238 0.0959739 0.0924656 0.0971976 0.0760233 0.0605125 0.0632098 0.0663649 0.0625042 0.0432244 0.0727551 0.0118371 0.0482469 0.0328974 0.00182117 0.0399752 -0.000438184 0.0700311 0.0914908 0.0258494 0.0211822 0.0172613 0.108348 0.131485 0.181341 0.109958 0.103511 0.222677 0.165348 0.112233 0.115058 0.131418 0.170228 0.110305 0.113631 0.215512 0.172329 0.0916719 0.0936136 0.119494 0.0681973 0.0325379 0.133076 0.157286 0.0852198 0.0751159 0.0870103 0.0564823 0.089638 0.142028 0.136605 0.115762 0.124376 0.122113 0.0991726 0.167862 0.153723 0.145996 0.15982 0.115621 0.228886 0.149941 0.145206 0.105554 0.125434 0.0925286 0.107219 0.0935523 0.0522544 0.0835663 0.0663668 0.0881322 0.125653 0.104988 0.102528 0.061023 0.0231979 -0.00853741 -0.0137216 -0.00584621 -0.00728291 -0.0146233 0.020795 0.0316677 0.0694413 0.112188 0.0841144 0.125354 0.103556 0.177539 0.170419 0.229773 0.20959 0.158388 0.0928798 0.144142 0.120969 0.06655 0.0289908 0.0125791 0.0528102 0.098517 0.0932281 0.100951 0.0374551 0.021861 0.0457135 0.128854 0.0689673 0.0437416 0.0994274 0.115351 0.0900772 0.0708663 0.123205 0.0943284 0.0514199 0.103428 0.0976305 0.106788 0.0579052 0.166993 0.0787556 0.044211 0.0378427 0.0529261 0.0521438 0.0806847 0.0573286 0.0576224 0.12918 0.100967 0.0972997 0.0843451 0.128208 0.184608 0.127432 0.0907981 0.075814 0.0633072 0.0903455 0.0580656 0.127782 0.0641255 0.0821403 0.0730126 0.0418176 0.0192085 0.00743078 0.0356362 0.095374 0.0821042 0.110676 0.115852 0.113115 0.0605605 0.106006 0.135064 0.10172 0.104364 0.136508 0.126788 0.215612 0.194969 0.114163 0.0762272 0.0954404 0.106911 0.0935199 0.0986577 0.0910032 0.0849372 0.119896 0.105758 0.123109 0.0814456 0.072276 0.0875083 0.0997572 0.0664407 0.0459355 0.0354643 0.00882343 -0.0119747 -0.0133528 -0.0181203 -0.0160952 -0.0167176 -0.0133434 -0.00746816 -0.0245515 -0.0504913 0.0440475 0.12867 0.146836 0.0800382 0.0669136 0.0243988 0.0629043 0.0820174 0.0476452 0.0795292 0.0971617 0.0982545 0.106496 0.101696 0.111744 0.107219 0.09382 0.0914172 0.0986696 0.0942122 0.147153 0.179134 0.147439 0.163253 0.123342 0.0873109 0.0799037 0.0874754 0.0864577 0.0802027 0.054995 0.0665705 0.0859635 0.0857601 0.0722152 0.0555832 0.122002 0.143991 0.102666 0.0738044 0.0840727 0.072307 0.155547 0.0858487 0.0707698 0.0597133 0.0324935 0.0557088 0.0352628 0.10161 0.0960705 0.100467 0.0591693 0.0321458 0.0341746 0.0728066 0.10087 0.0524515 0.147672 0.136329 0.085361 0.0939264 0.146584 0.0865834 0.0629399 0.0492386 0.0590409 0.0687015 0.0583297 0.0346241 0.0859408 0.0783146 0.0745072 0.0890257 0.131418 0.104168 0.117213 0.0733527 0.0569845 0.0572538 0.065091 0.044685 0.0566213 0.0791148 0.104937 0.0952604 0.132849 0.129148 0.225025 0.118795 0.0770543 0.0754116 0.0811417 0.0518886 0.0970446 0.0926451 0.132011 0.078656 0.0676844 0.063011 0.0685007 0.0750278 0.0763491 0.0992065 0.0837295 0.0389351 0.0679432 0.0919818 0.0816956 0.108031 0.163553 0.147272 0.0801326 0.0510047 0.13931 0.104258 0.109869 0.0740939 0.0593046 0.052944 0.102768 0.0889514 0.088116 0.0644777 0.107473 0.059841 0.0478006 0.0545327 0.0807842 0.0507364 0.0460261 0.0463611 0.0041533 -0.0112511 -0.00182496 0.029502 0.0875185 0.0367835 0.0430241 0.136813 0.154237 0.128942 0.106829 0.0875528 0.0556126 0.0457232 0.0553136 0.0366947 0.0525837 0.03713 0.0509924 0.0359717 0.0970523 0.057306 0.0579845 0.0656516 0.0667005 0.0961338 0.135493 0.104492 0.109033 0.0659016 0.057813 0.0673877 0.0227377 -0.0253198 0.0310285 -0.0231604 -0.0329342 -0.0386423 -0.00694487 0.0312054 0.00963072 -0.00695971 -0.00749498 0.00426875 -0.021462 -0.00702565 -0.0570642 -0.108699 -0.104231 -0.127644 -0.144276 -0.131265 -0.153328 -0.136342 -0.0907522 0.000354443 0.0199026 -0.0197584 -0.0666496 -0.102117 -0.0787243 -0.0087891 0.018622 0.0830861 0.0286004 0.00670091 0.066298 0.0902445 0.0291831 0.0247759 0.0166041 0.0520078 0.0749008 0.0976591 0.127767 0.163109 0.196154 0.161337 0.16272 0.177426 0.193231 0.22037 0.225091 0.19891 0.201018 0.169057 0.163643 0.156295 0.122732 0.0969857 0.0551523 0.0963938 0.0639662 0.0385599 0.0451742 0.0364716 0.0202491 0.00340754 0.0190544 0.0523586 0.0635982 0.0829982 0.065111 0.0540254 0.0978546 0.104197 0.118081 0.204906 0.207036 0.184061 0.133468 0.0882952 0.131427 0.150059 0.203749 0.140562 0.109586 0.0863679 0.0864655 0.0539874 0.0747149 0.0736916 0.0452751 0.05527 0.0256688 0.0354098 0.01028 -0.0222459 0.0560329 -0.021644 -0.058415 -0.0454518 -0.0343694 -0.0421567 -0.0423416 -0.0642484 -0.0445514 -0.0453998 -0.00941682 -0.0488264 -0.0686027 -0.0611836 -0.0954985 -0.111791 -0.123832 -0.139467 -0.14163 -0.177082 -0.231342 -0.312842 -0.354024 -0.395208 -0.481271 -0.62408 -0.67845 -0.821495 -0.876881 -0.896471 -0.922166 -0.975289 -1.04436 -1.06946 -1.0996 -1.17399 -1.20603 -1.26501 -1.29627 -1.33037 -1.36186 -1.4216 -1.45968 -1.51013 -1.50993 -1.51303 -1.46751 -1.48185 -1.48874 -1.5171 -1.55625 -1.58049 -1.54476 -1.5696 -1.59112 -1.61319 -1.59619 -1.6039 -1.58078 -1.54869 -1.49992 -1.50747 -1.49621 -1.4894 -1.49796 -1.50966 -1.46446 -1.43156 -1.42642 -1.40603 -1.43262 -1.44425 -1.45731 -1.49124 -1.5324 -1.54775 -1.55892 -1.57561 -1.58283 -1.61081 -1.63664 -1.59961 -1.63024 -1.63185 -1.67511 -1.68876 -1.70205 -1.69109 -1.70205 -1.71196 -1.70491 -1.71343 -1.72928 -1.72688 -1.78051 -1.76211 -1.75061 -1.81361 -1.76787 -1.7241 -1.71791 -1.68638 -1.66685 -1.68219 -1.66915 -1.65274 -1.6385 -1.53327 -1.46295 -1.44572 -1.38359 -1.38176 -1.40857 -1.3769 -1.26487 -1.1807 -1.07709 -0.964294 -0.954541 -0.934682 -0.847316 -0.859035 -0.788509 -0.80117 -0.687314 -0.611245 -0.615024 -0.489901 -0.401035 -0.360056 -0.354159 -0.355181 -0.287055 -0.181826 -0.176819 -0.176513 -0.141123 -0.0632313 -0.0842116 -0.0837222 -0.0760314 -0.0584509 -0.0916075 -0.0700764 -0.0778431 -0.0200633 -0.0502082 -0.0596606 -0.0511292 -0.0134961 -0.0495379 -0.0902032 -0.119074 -0.0616637 -0.0738025 -0.101675 -0.187184 -0.302752 -0.414086 -0.495903 -0.488898 -0.597248 -0.746857 -0.784694 -0.877277 -1.00734 -1.03921 -1.15152 -1.19673 -1.28701 -1.38024 -1.46971 -1.56005 -1.62789 -1.69639 -1.69164 -1.70703 -1.79194 -1.80627 -1.91986 -1.98407 -2.00382 -2.00921 -1.98767 -2.03754 -2.06141 -2.05511 -2.07779 -2.12165 -2.10421 -2.07396 -2.01418 -2.0603 -2.06734 -1.97036 -1.90456 -1.86372 -1.86958 -1.78454 -1.70381 -1.71959 -1.67096 -1.63917 -1.7209 -1.83239 -1.82299 -1.86495 -1.89255 -1.83686 -1.76546 -1.77285 -1.83561 -1.83031 -1.8596 -1.98766 -1.8702 -1.77089 -1.861 -1.79824 -1.77306 -1.68802 -1.70956 -1.67134 -1.60673 -1.57674 -1.47405 -1.29696 -1.15566 -1.0267 -0.999502 -0.951971 -0.970031 -0.909957 -0.723638 -0.573486 -0.358049 -0.329686 -0.226238 -0.120295 -0.123382 -0.163603 -0.142851 -0.146428 -0.155925 -0.0870104 -0.115286 -0.148371];
yaw = yaw * pi / 180; % to radian
yaw_filtered = pf_filter(yaw, particles, noise, lambda);
plot(1:length(yaw),yaw,'b', 1:length(yaw),yaw_filtered,'r');
legend('yaw', 'yaw filtered')

%city
subplot(212)
yaw=[0.30475 0.179487 0.174259 0.188402 0.138077 0.022115 -0.0226229 -0.0376746 -0.0132458 -0.00982783 -0.0117854 0.0181216 0.0245482 0.022687 -0.0140762 -0.0070166 -0.0337251 -0.00385072 -0.0177757 0.0238846 0.0508573 0.0704911 0.0589712 0.0758277 0.0801716 0.104393 0.137757 0.181857 0.215861 0.279734 0.321753 0.36373 0.29628 0.230096 0.139928 0.0528432 0.00396209 -0.040203 -0.0564339 -0.0556133 -0.0756455 -0.0736227 -0.0801889 -0.078539 -0.0756008 -0.0496154 -0.0430346 -0.0245262 -0.0235629 -0.0306748 -0.0209008 -0.0254569 -0.0163843 0.00961225 0.0873266 0.176322 0.258747 0.358785 0.408354 0.411247 0.343477 0.252625 0.134853 0.0533267 -0.0270421 -0.103341 -0.192176 -0.297755 -0.33782 -0.444003 -0.505229 -0.603846 -0.628409 -0.638968 -0.619463 -0.590039 -0.504881 -0.391696 -0.249847 -0.102476 -0.0290698 0.0557653 0.0850319 0.128833 0.15877 0.212619 0.229911 0.234608 0.237816 0.218427 0.190242 0.156444 0.095595 0.0899091 0.0894013 0.0683385 0.0438716 0.0441656 0.00920117 -0.0359666 -0.0195908 0.0150873 -0.0224601 0.00344401 -0.00325148 0.0031264 0.0342783 0.0225626 0.00125394 0.00590956 0.0266414 0.059555 0.0781169 0.0915641 0.102397 0.09782 0.106186 0.0952049 0.125242 0.112837 0.09765 0.069482 0.0650946 0.024309 -0.00276194 0.000521152 -0.0500477 -0.129134 -0.185894 -0.233008 -0.292637 -0.337297 -0.394273 -0.453911 -0.506018 -0.559811 -0.597655 -0.608499 -0.677162 -0.710101 -0.865954 -0.991408 -1.13163 -1.33966 -1.55355 -1.63137 -1.74566 -1.91829 -2 -2.10153 -2.28746 -2.47415 -2.55137 -2.64716 -2.88901 -3.02999 -3.32579 -3.67924 -3.92469 -4.09015 -4.17156 -4.15607 -4.22473 -4.28558 -4.25375 -4.1892 -4.0245 -3.73987 -3.42894 -3.25172 -3.04481 -2.74181 -2.46118 -2.27442 -2.01925 -1.75486 -1.50075 -1.2718 -1.02958 -0.893209 -0.732964 -0.613297 -0.525331 -0.474929 -0.387553 -0.305334 -0.258558 -0.189068 -0.13018 -0.0676081 -0.0216887 0.00640681 0.0399184 0.0688367 0.0691835 0.0937832 0.0950576 0.0864711 0.0849131 0.146693 0.171651 0.147389 0.13547 0.131069 0.139342 0.157881 0.169127 0.177691 0.24644 0.30326 0.282388 0.21072 0.148338 0.115538 0.0973242 0.0903632 0.0203192 -0.127068 -0.201362 -0.198261 -0.183368 -0.100807 0.0286894 0.136901 0.210031 0.285578 0.343586 0.428124 0.542958 0.599242 0.634337 0.644743 0.64029 0.603393 0.534696 0.423446 0.300559 0.190319 0.0690183 -0.0284393 -0.0784041 -0.130472 -0.12986 -0.123298 -0.144194 -0.140995 -0.127025 -0.111517 -0.106797 -0.102712 -0.101239 -0.0628823 -0.0647643 -0.0849767 -0.0552471 -0.010536 0.0253839 0.0461297 0.0628654 0.0585836 0.0873507 0.159151 0.199473 0.227096 0.230369 0.246443 0.221712 0.250071 0.226917 0.211861 0.204407 0.206682 0.224886 0.23281 0.223546 0.227515 0.238515 0.229102 0.22341 0.234811 0.245099 0.258166 0.285658 0.328705 0.363733 0.382328 0.383369 0.369634 0.35895 0.324807 0.28621 0.278117 0.253206 0.242337 0.197246 0.181175 0.150852 0.126945 0.143503 0.119872 0.116917 0.119716 0.112794 0.108205 0.0974032 0.086502 0.098268 0.0818297 0.10165 0.0892757 0.0847187 0.0869966 0.0908422 0.106367 0.0996518 0.135482 0.121813 0.142016 0.143964 0.147656 0.119127 0.0909756 0.0990453 0.0828671 0.0869893 0.0632103 0.0702175 0.0797907 0.048776 0.057801 0.0498222 0.038227 0.0572239 0.0609147 0.0659101 0.0773921 0.0362802 0.0494541 0.0540135 0.0776939 0.0469305 0.0557326 0.0740368 0.0729994 0.10053 0.0966857 0.112374 0.134886 0.103594 0.119675 0.177963 0.185633 0.178445 0.212828 0.286304 0.301076 0.268763 0.273073 0.226959 0.199618 0.164198 0.136814 0.110538 0.145624 0.0994131 0.0215263 0.0690092 0.0837448 -0.00840844 -0.00521579 0.0183814 -0.0181323 -0.0630966 -0.0578833 -0.0652433 -0.104919 -0.109118 -0.120956 -0.147402 -0.159655 -0.188896 -0.182363 -0.200286 -0.210299 -0.212779 -0.236165 -0.200242 -0.158933 -0.109148 -0.0857401 -0.0544715 -0.0449779 -0.0297336 -0.0319142 -0.0225307 -0.0224586 -0.000272191 0.0125224 0.0305315 0.0368781 0.040263 0.0690737 0.0469163 0.0247968 -0.0160182 -0.0334243 -0.020143 -0.0258138 -0.0120463 -0.0270127 -0.0223702 -0.0515498 -0.0523407 -0.0467518 -0.00399613 0.0236801 0.0643724 0.0977556 0.0907296 0.0858683 0.0751137 0.0660724 0.0164567 -0.0238309 -0.0508525 -0.0325317 -0.0512124 -0.0511515 -0.0624202 -0.0390971 -0.0280026 -0.00100654 0.000741208 0.0198076 0.026281 0.038335 0.0373232 0.0273147 0.0593405 0.0702573 0.0622719 0.0575668 0.0556377 0.0542898 0.0679694 0.0593302 0.0453422 0.0498113 0.0140518 -0.0111507 -0.0282021 -0.0802214 -0.102427 -0.0930403 -0.0684336 -0.0157247 -0.020047 0.00132737 0.0176 0.0600413 0.115652 0.0814265 0.102194 0.0966553 0.108734 0.112235 0.110216 0.0940424 0.11577 0.192258 0.083762 0.0803321 0.113454 0.0921114 0.0825793 0.100536 0.0516432 0.0812471 -0.0238479 -0.0998327 -0.12542 -0.213911 -0.364066 -0.473139 -0.624058 -0.697938 -0.871303 -0.982245 -1.06793 -1.28381 -1.40844 -1.56725 -1.6319 -1.68972 -1.71769 -1.68367 -1.68403 -1.69456 -1.63639 -1.52253 -1.45692 -1.35002 -1.29465 -1.13142 -1.04716 -0.939912 -0.920201 -0.880556 -0.816902 -0.871844 -0.852363 -0.796609 -0.752464 -0.773219 -0.793182 -0.802711 -0.835196 -0.827361 -0.832698 -0.874853 -0.965853 -1.03543 -1.1337 -1.24735 -1.35515 -1.46937 -1.52231 -1.60111 -1.6958 -1.68771 -1.70665 -1.77504 -1.82342 -1.95001 -2.06892 -2.1778 -2.19166 -2.17841 -2.22685 -2.39464 -2.58643 -2.74919 -2.768 -2.82353 -2.93836 -3.01711 -3.03533 -3.0116 -2.98513 -2.96639 -2.93876 -2.90006 -2.70309 -2.38017 -2.03941 -1.75989 -1.52349 -1.40134 -1.37157 -1.23595 -0.944269 -0.684709 -0.45057 -0.181947 -0.0161365 0.128022 0.122816 0.151083 0.197293 0.246717 0.244396 0.234753 0.163848 0.0964014 0.0228479 0.00162883 0.00283404 -0.0122809 0.00208193 0.00749934 0.00399346 0.0167406 0.0346344 0.0341959 0.0140177 0.0112057 0.0228504 0.0193062 0.0192973 -0.00144839 -0.00559948 0.0173445 0.00781053 -0.00482946 0.00247927 0.000497694 0.0263564 0.0552863 0.0685315 0.0825042 0.0817904 0.0869118 0.0872751 0.0926708 0.0893405 0.0740116 0.0565717 0.0717141 0.0720801 0.0409948 0.0605736 0.0519228 0.0618244 0.0666852 0.0644941 0.0714841 0.0821545 0.00881515 0.0522865 0.0155909 -0.0196378 -0.095571 -0.0790865 -0.090129 -0.0658561 -0.0595764 -0.0450493 0.0179852 0.0595287 0.0574478 0.0531096 0.082733 0.0852621 0.097679 0.0792456 0.0526006 -0.00320593 -0.0327667 -0.098492 -0.116485 -0.168193 -0.174509 -0.221793 -0.260425 -0.303693 -0.300422 -0.314461 -0.308329 -0.295453 -0.270887 -0.238395 -0.199471 -0.19248 -0.215126 -0.187012 -0.188298 -0.154654 -0.125579 -0.110278 -0.0521341 0.00721563 0.0259175 0.101456 0.158041 0.201599 0.186861 0.158562 0.257548 0.262711 0.218567 0.244517 0.219861 0.254497 0.266459 0.218654 0.195917 0.153183 0.116148 0.0864638 0.0609964 0.0380265 0.0338814 0.0727005 0.146663 0.180008 0.197424 0.211784 0.214519 0.168377 0.141434 0.100064 0.0906542 0.084288 0.0556896 0.0202406 -0.0617684 -0.126539 -0.18495 -0.164601 -0.188335 -0.20503 -0.200944 -0.181864 -0.194855 -0.198112 -0.201875 -0.19328 -0.162312 -0.111446 -0.0482667 -0.0214905 -0.062493 -0.0437037 -0.0252648 -0.0235096 -0.0214863 -0.0235655 -0.0386551 -0.030296 -0.0697825 -0.0668956 -0.0945167 -0.158501 -0.205618 -0.298448 -0.385339 -0.426002 -0.517342 -0.629108 -0.751786 -0.830309 -0.882969 -0.915117 -1.0268 -1.17683 -1.32996 -1.48134 -1.64584 -1.80987 -1.93981 -2.08522 -2.17321 -2.21989 -2.2842 -2.3835 -2.45825 -2.54834 -2.69141 -2.8187 -2.90245 -2.93296 -2.97633 -2.96088 -2.90341 -2.89212 -2.85744 -3.04533 -3.11478 -2.93961 -2.73138 -2.78172 -2.88271 -3.00246 -2.91201 -2.88718 -2.80271 -2.82662 -2.83338 -2.83243 -2.77083 -2.78791 -2.80746 -2.80193 -2.82525 -2.82571 -2.85774 -2.83164 -2.68467 -2.61558 -2.49123 -2.45428 -2.51835 -2.42179 -2.19455 -1.95691 -1.75477 -1.43262 -1.16669 -1.05163 -0.923694 -0.667625 -0.462237 -0.311442 -0.23215 -0.132824 -0.0331087 -0.0172291 0.00093105 0.0694759 0.0753771 0.0726006 0.12644 0.109008 0.1095 0.128574 0.076494 0.061131 0.0514758 0.0329612 0.0576562 0.0573599 0.0818912 0.0861914 0.0842703 0.0952984 0.0890221 0.101377 0.104476 0.107198 0.122077 0.117813 0.110013 0.113439 0.122016 0.0766178 0.0578684 -0.00509901 -0.0820416 -0.158081 -0.261762 -0.43225 -0.591253 -0.729793 -0.814679 -0.821426 -0.942902 -1.06243 -1.09979 -1.17284 -1.37936 -1.53551 -1.64681 -1.72577 -1.85715 -1.91333 -2.06603 -2.28098 -2.47848 -2.50784 -2.64661 -2.76615 -2.75389 -2.80701 -2.94119 -2.95696 -2.95768 -2.85181 -2.8998 -2.93062 -2.92871 -2.92124 -2.88285 -2.85659 -2.76478 -2.58167 -2.42774 -2.30687 -2.09989 -1.91236 -1.65728 -1.33886 -1.12668 -0.992689 -0.877605 -0.858959 -0.82066 -0.658231 -0.501184 -0.381274 -0.24806 -0.110229 -0.0796144 -0.0838291 -0.0217668 -0.0238102 0.0297624 0.0567981 0.0278395 0.0626159 0.0779291 0.0702471 0.0958635 0.0638707 0.103998 0.108367 0.101456 0.0971582 0.0801272 0.110849 0.0624149 0.0294443 0.0258563 0.0494425 0.0337489 0.0325763 0.0792641 0.0704389 0.0838592 0.0732751 0.0727977 0.138281 0.203682 0.148725 0.0196158 -0.102502 -0.161492 -0.20265 -0.214768 -0.234472 -0.206814 -0.203046 -0.143617 -0.0984808 -0.0586795 -0.0648867 0.001561 0.062582 0.0837579 0.0672667 0.0745028 0.0745618 0.0849063 0.0461263 0.0622535 0.0635539 0.055498 0.0376198 0.00875422 0.02267 0.0521593 0.0723478 0.0202669 0.0161487 0.015058 0.0250308 -0.0305151 -0.0110266 0.00440461 0.000313256 -0.0139613 -0.0179996 0.0319308 -0.00176931 0.000573998 -0.0110942 -0.017804 -0.0206135 -0.00243792 -0.0108519 -0.00510409 -0.0103345 -0.0230042 -0.0212024 -0.0010213 -0.00101371 0.0103855 0.0273124 0.0700086 0.0410119 0.0469843 0.0768077 0.0834133 0.119673 0.0696013 0.0631381 0.0543344 0.0271923 0.0254546 -0.00613975 0.0283279 -0.00970605 -0.00825208 0.0026909 -0.0136063 0.0208149 0.0146968 -0.0274485 -0.0105587 -0.0238021 0.0122282 -0.00546018 0.0337979 -0.0127771 -0.048599 -0.066124 -0.125863 -0.147585 -0.166121 -0.168688 -0.148124 -0.158528 -0.134196 -0.120654 -0.13196 -0.117803 -0.121518 -0.105335 -0.120771 -0.122413 -0.111552 -0.0971404 -0.0755905 -0.0557986 -0.00611884 0.0238639 0.0312592 0.0602 0.0737668 0.084973 0.0284151 -0.0124846 -0.0231491 -0.052921 -0.0712247 -0.041036 -0.0368411 -0.0437675 -0.0554429 -0.054619 -0.0306885 0.0193076 0.0354945 0.0257321 0.0546866 0.0420957 0.054954 0.0555795 0.0563493 0.0465214 0.0129101 -0.0258873 -0.0353074 -0.0334062 -0.0671033 -0.101623 -0.100301 -0.0993492 -0.106734 -0.147148 -0.115431 -0.0944534 -0.0547475 -0.0514065 0.00163841 -0.00317043 -0.00195109 0.036409 0.0218104 0.01945 -0.0191361 -0.00760283 -0.00279505 0.0342112 0.0570132 0.0588352 0.0514045 0.0651947 0.0555983 0.0512073 0.0399205 0.040154 0.0357353 0.0640871 0.0170143 -0.00758046 -0.0169547 -0.013437 -0.0427757 -0.0370036 -0.0456435 -0.0349145 -0.0336431 -0.0230159 0.00376306 0.011186 0.005552 0.0224672 0.0103297 0.0141336 0.0275613 0.0692004 0.072925 0.0475791 0.029414 0.0428894 0.021778 0.0193846 0.00374757 0.0268328 -0.00703684 -0.0424315 -0.067187 -0.0595433 -0.0760761 -0.0822113 -0.081541 -0.0723966 -0.061317 -0.0526922 -0.0528767 -0.0591115 -0.0878899 -0.0976713 -0.0830524 -0.0850117 -0.0681988 -0.0783386 -0.075997 -0.0840638 -0.0504191 -0.0498459 0.0316384 0.0918179 0.154363 0.252641 0.456718 0.710352 0.957216 1.24292 1.66217 2.19337 2.61853 2.97802 3.30919 3.55614 3.7273 3.92771 4.09718 4.21795 4.317 4.43569 4.45933 4.49508 4.51647 4.53744 4.49271 4.36521 4.15193 3.82313 3.42754 3.11403 2.79118 2.34195 1.91464 1.60146 1.33256 0.993947 0.766277 0.729033 0.705833 0.57438 0.498207 0.392068 0.290598 0.259344 0.210964 0.209317 0.145339 0.0936843 0.0807901 0.0925898 0.0813617 0.0589477 0.0525443 0.0518243 0.0371227 0.0351677 0.0479786 0.0257362 0.0092561 -0.0248859 -0.00690028 -0.033877 -0.0244459 -0.0239138 -0.0339938 -0.0322619 -0.0445283 -0.0458071 -0.0432014 -0.010657 -0.0201027 -0.0427703 -0.00647301 0.0323205 0.0830555 0.0996489 0.085334 0.0876044 0.138108 0.118005 0.137313 0.117213 0.0926589 0.0610482 0.0184387 0.0126472 -0.0211238 -0.0763662 -0.129032 -0.110208 -0.0632401 -0.00265423 0.0577395 0.0117093 0.108191 -0.00130269 -0.0546606 -0.0776101 -0.0576762 -0.0566674 -0.00577195 -0.0164941 -0.0207703 -0.0493278 -0.0181145 -0.0114736 0.0170216 0.0455068 0.0939253 0.108352 0.116523 0.111777 0.0187287 -0.0217404 -0.0812307 -0.0728245 -0.0668284 -0.0839288 -0.0691943 -0.0896208 -0.0765671 -0.0746829 -0.0707666 -0.0773607 -0.101656 -0.149621 -0.163576 -0.169688 -0.179351 -0.19268 -0.196981 -0.244301 -0.242042 -0.261783 -0.254327 -0.27373 -0.269438 -0.267431 -0.261209 -0.242382 -0.240403 -0.210783 -0.155636 -0.118368 -0.0501729 -0.00161248 0.0651181 0.122528 0.147234 0.197524 0.229417 0.300325 0.308113 0.349337 0.305951 0.236083 0.17354 0.213093 0.138131 0.111922 0.121805 0.123242 0.100957 0.0734994 0.0319933 -0.0074415 -0.0643405 -0.0696537 -0.0950914 -0.115293 -0.103581 -0.0751823 0.0365246 -0.067284 -0.083865 -0.0811351 -0.00626573 0.0302736 0.0249664 0.00880526 -0.00100666 0.0108548 0.00718117 -0.0185468 0.0105581 0.00381042 0.0272406 0.0297883 0.0292608 0.0613662 0.0648209 0.0895853 0.0646205 0.0838738 0.0891583 0.0625029 0.0681595 0.0486759 0.0577643 0.0635325 0.0304086 0.0417993 0.0137969 0.0334629 -0.0151836 -0.0268802 -0.0771564 -0.0913431 -0.0915014 -0.108294 -0.0512946 -0.0804498 -0.0676161 -0.0340894 -0.0835094 -0.0425789 0.00596008 0.0133769 0.0242728 -0.0159059 0.0155589 -0.00128625 -0.0150575 0.0280088 0.0266521 0.00936855 0.0166 0.029112 0.0281173 0.0530989 0.043178 -0.000884305 -0.0156495 0.0690506 0.0246055 0.0059376 -0.0307096 -0.0211329 0.0384577 0.0768438 0.107916 0.0814109 0.1025 0.122411 0.0771798 0.0936294 0.0856521 0.0968666 0.121836 0.0936304 0.118932 0.0797088 0.0450468 0.0180372 0.0331664 0.0342488 0.0927842 0.0725752 0.0695621 0.101953 0.15841 0.162579 0.154598 0.139402 0.153884 0.155881 0.135673 0.12608 0.119265 0.100479 0.111504 0.104705 0.0868163 0.06118 0.0285746 0.0361625 0.0316795 0.0344985 -0.0139041 -0.0699331 -0.125755 -0.14858 -0.13273 -0.161601 -0.228676 -0.245095 -0.253778 -0.324093 -0.327891 -0.237334 -0.200719 -0.19503 -0.151476 -0.112496 -0.104677 -0.0464988 0.00257339 0.0711086 0.187206 0.380506 0.560086 0.758684 0.922132 1.0056 1.10402 1.19526 1.29578 1.45197 1.62976 1.87659 1.99915 2.16293 2.31429 2.43084 2.56126 2.57134 2.55341 2.79954 2.80006 2.5946 2.71278 3.00393 2.73191 2.55927 2.54612 2.53019 2.47495 2.54535 2.59869 2.60001 2.60485 2.51186 2.49149 2.56003 2.4671 2.48202 2.59691 2.58576 2.67846 2.8494 3.00757 3.15023 3.25875 3.29521 3.29052 3.31431 3.28316 3.17156 2.99752 2.71314 2.32363 1.94364 1.67321 1.46782 1.23154 0.982308 0.780634 0.629013 0.5012 0.329321 0.156877 0.0872717 0.0563897 0.0535618 0.0480943 0.110665 0.183913 0.215187 0.172213 0.16303 0.137822 0.132053 0.0976242 0.11866 0.0770669 0.0754421 0.0458249 0.0144459 -0.0231456 -0.000447319 -0.00328453 -0.0396934 -0.0187803 -0.0274635 0.000624928 -0.0189377 -0.0317731 -0.0164748 -0.0176673 -0.00663377 -0.0136971 -0.0238614 -0.0148498 -0.0328094 -0.0464577 -0.0559506 -0.0369467 -0.0396434 -0.0288519 -0.0209106 -0.0140712 0.00329739 0.0176055 0.0320681 0.0496328 0.0602179 0.0833475 0.0848271 0.0848344 0.0530653 0.0389138 0.0172393 0.0143637 0.0117783 0.00677643 -0.00567854 -0.0169066 -0.0213849 -0.00959711 -0.00632405 -0.00545317 0.00158795 0.0332752 0.0641731 0.0603015 0.0934607 0.0850019 0.0951686 0.0994157 0.150355 0.15063 0.158065 0.151901 0.146782 0.155163 0.145008 0.126112 0.125763 0.120029 0.113378 0.114736 0.123228 0.136145 0.115333 0.110737 0.0918361 0.0660385 0.0466763 0.0150914 -0.0164442 -0.0270084 -0.0291673 -0.0346205 -0.0314821 0.0257207 0.0551511 0.0404325 0.0181875 0.00930459 -0.0173557 -0.0854018 -0.128856 -0.142866 -0.168803 -0.197602 -0.260526 -0.316691 -0.4324 -0.52729 -0.587855 -0.611173 -0.614316 -0.640127 -0.695656 -0.767722 -0.857467 -0.918413 -0.945898 -0.9972 -0.822677 -0.72002 -0.638766 -0.549073 -0.433319 -0.297952 -0.202444 -0.15174 -0.0524001 0.0694834 0.24705 0.397566 0.510239 0.594385 0.675179 0.754705 0.816802 0.805847 0.726108 0.619943 0.514495 0.447792 0.430201 0.481008 0.472249 0.467529 0.440818 0.416274 0.359801 0.216877 0.0949137 0.0591895 0.0674334 0.0500264 -0.015628 -0.0808674 -0.111236 -0.0620003 0.0464784 0.120837 0.205308 0.276574 0.292437 0.254779 0.222913 0.176204 0.172714 0.146858 0.140862 0.153585 0.108384 0.120786 0.100533 0.109936 0.103899 0.0741799 0.0583946 -0.0132118 -0.0160518 -0.0500703 -0.0514484 -0.0811561 -0.0891832 -0.111667 -0.0794393 -0.0754348 -0.0928282 -0.104925 -0.10787 -0.0686013 -0.0478312 -0.00140932 0.00963712 -0.0110372 0.000389037 0.0182985 0.0213251 0.0273668 0.0152716 0.000841995 0.0111123 -0.0155829 0.0211788 0.0224995 0.00796894 0.00956949 0.00300651 -0.00751289 -0.0289531 -0.0768751 -0.0742896 -0.0487655 -0.0538047 -0.0350236 -0.0324171 -0.0170527 0.0162889 0.0147051 0.0269295 0.0262349 0.0272692 0.0385585 0.0325889 0.0342383 0.0192645 0.0244853 0.0272268 0.0347053 0.027941 0.0269363 0.0156148 0.0128438 0.00549403 -9.26619e-05 0.00275127 -0.00919595 0.00932689 0.00559923 -0.00659993 0.0119755 0.0205586 0.0104374 -0.00908094 -0.0199308 -0.0575303 -0.0875079 -0.0888103 -0.0822947 -0.0457444 -0.107912 -0.146803 -0.219009 -0.235113 -0.225403 -0.22951 -0.25158 -0.257082 -0.278608 -0.279673 -0.261406 -0.280933 -0.248255 -0.204421 -0.113599 -0.0532836 -0.015232 -0.0136706 0.0312096 0.00405776 0.0587149 0.0614592 0.106957 0.131174 0.129629 0.0593068 0.0840759 0.194596 0.21212 0.176563 0.17026 0.21443 0.2452 0.237812 0.255399 0.177533 0.172497 0.177954 0.170644 0.187654 0.176687 0.136773 0.099697 0.0501681 0.0990627 0.0975259 0.0600685 0.0913272 0.10044 0.161236 0.172275 0.166524 0.154279 0.117818 0.0473685 -0.00306168 -0.044007 -0.0826237 -0.15383 -0.149233 -0.163632 -0.149289 -0.209926 -0.232402 -0.227346 -0.220074 -0.203994 -0.172792 -0.169357 -0.133851 -0.136515 -0.10851 -0.0793078 -0.063107 -0.0742153 -0.0783069 -0.047635 -0.0409103 -0.0312231 -0.0259114 -0.0039658 -0.0164047 0.00365514 -0.0583416 -0.0834261 -0.156837 -0.215367 -0.211919 -0.287264 -0.390132 -0.465833 -0.508875 -0.578999 -0.680661 -0.748488 -0.816078 -0.837852 -0.880837 -0.967129 -1.07209 -1.20592 -1.33482 -1.50214 -1.70858 -1.8566 -1.96876 -2.11019 -2.25928 -2.43055 -2.59855 -2.85219 -3.11377 -3.23573 -3.25751 -3.19156 -3.1521 -3.19236 -3.16071 -3.08848 -3.04097 -2.92593 -3.08076 -2.98658 -2.73009 -2.76883 -2.86461 -2.85749 -2.92734 -2.95235 -2.90737 -2.8262 -2.86643 -2.91262 -2.88386 -2.84935 -2.80807 -2.78341 -2.70911 -2.69038 -2.59618 -2.50286 -2.48902 -2.38748 -2.27588 -2.14328 -1.9743 -1.89094 -1.78047 -1.54028 -1.34504 -1.25105 -1.17065 -1.07309 -0.887885 -0.841967 -0.878798 -0.821739 -0.724128 -0.683347 -0.54566 -0.396853 -0.289364 -0.127017 -0.0179304 0.0731243 0.0786512 0.0616059 0.0944952 0.103209 0.101872 0.110359 0.0838209 0.0557865 0.0251347 -0.0112115 -0.0228181 -0.0177158 -0.0263354 -0.00970592 -0.00458721 -0.00198991 -0.0031303 -0.0116559 -0.0169328 -0.0558692 -0.00463992 -0.0235385 -0.0071126 -0.0197248 -0.0270052 -0.0198055 -0.017981 -0.0284635 -0.00217309 -0.0255355 -0.0720195 -0.0636431 -0.0753342 -0.079348 -0.109932 -0.153031 -0.214134 -0.245698 -0.306142 -0.303813 -0.276492 -0.219899 -0.280505 -0.242044 -0.198138 -0.177393 -0.127971 -0.0622502 0.0525779 0.190511 0.387733 0.627878 0.846311 1.01693 1.22906 1.44649 1.55705 1.5834 1.67693 1.82029 1.95935 2.05377 2.16415 2.31552 2.36776 2.61226 2.70983 2.76935 2.89655 2.96955 3.00918 3.12255 3.25061 3.3666 3.44468 3.39387 3.3935 3.41047 3.3108 3.14651 2.93815 2.66926 2.36941 2.14893 1.88454 1.69686 1.45955 1.19959 1.03793 1.0323 1.05791 0.993317 0.961054 0.953963 0.836977 0.784775 0.712373 0.612928 0.530611 0.419257 0.415221 0.340606 0.390508 0.310196 0.248483 0.17849 0.193855 0.14724 0.143339 0.117929 0.0977309 0.0529013 -0.0107711 -0.0303898 -0.0701901 -0.0830927 -0.106259 -0.151267 -0.225867 -0.215202 -0.218666 -0.209687 -0.220313 -0.239652 -0.211325 -0.226658 -0.220767 -0.220846 -0.233007 -0.213554 -0.182509 -0.141739 -0.11492 -0.126737 -0.102686 -0.118034 -0.12717 -0.117313 -0.11235 -0.107857 -0.113531 -0.120204 -0.0981249 -0.0967103 -0.0783841 -0.0559013 0.0112852 0.0197805 0.0322025 0.0148213 0.0138495 0.013858 0.0247632 -0.0113278 -0.015826 -0.041227 -0.0162998 -0.0253099 -0.0131528 -0.0115434 -0.0222985 -0.0200774 -0.0162796 -0.00127609 -0.00222184 0.00306795 0.00281335 0.00578576 -0.0113582 0.0169272 0.0126669 0.0226785 0.0149695 0.0164732 0.00827168 0.0252676 0.0196191 0.0269267 0.0219435 0.0307189 0.0264813 0.0309238 0.0112991 0.00853232 0.0200918 0.0177621 0.00680899 0.00093167 0.026951 0.0279513 0.043548 0.0301218 0.018974 0.0131959 0.00412797 0.0139147 0.0182268 0.00635087 0.0260346 0.00594479 -0.0502168 -0.0617459 -0.0885278 -0.167521 -0.221757 -0.249089 -0.389953 -0.557024 -0.687329 -0.82902 -0.968379 -1.22178 -1.36711 -1.50614 -1.71167 -1.92929 -2.13762 -2.32057 -2.47996 -2.55348 -2.57454 -2.6258 -2.70589 -2.77734 -2.872 -2.97525 -3.00767 -3.0379 -3.06242 -3.07188 -3.15136 -3.21344 -3.22526 -3.1781 -3.20281 -3.22621 -3.141 -2.95811 -2.75552 -2.4302 -2.09839 -1.75702 -1.5008 -1.30738 -1.18113 -1.07222 -0.894982 -0.68535 -0.449687 -0.272012 -0.143604 0.0201803 0.0927581 0.110953 0.123002 0.124212 0.133205 0.129444 0.120343 0.0569245 0.0655439 0.0282369 0.00379961 0.00407802 -0.0342494 -0.0386935 -0.0409035 -0.040164 -0.0185999 -0.0418931 -0.0322776 -0.0276855 -0.0315567 -0.0168371 -0.0273699 -0.0054038 -0.00618057 0.00127757 0.00516364 0.0166477 0.0323421 0.0283341 0.0375679 0.0364754 0.0374608 0.0275849 0.0254669 0.0308121 0.0330265 0.0209301 -0.0148426 -0.0499462 -0.0308175 -0.030319 0.0085185 -0.0182147 -0.0197403 -0.018271 -0.039999 -0.0245746 -0.00931007 0.0103776 0.00669514 0.00883062 0.0182125 0.00237973 0.0334788 0.0588377 0.0739544 0.0284395 0.0507739 0.053439 0.0246626 -0.0924749 -0.126284 -0.131356 -0.141071 -0.168718 -0.21469 -0.211787 -0.217986 -0.197032 -0.109125 -0.00442299 0.0617277 0.119886 0.182234 0.272195 0.401636 0.536023 0.628493 0.699317 0.725693 0.713033 0.734544 0.743832 0.704501 0.677641 0.625327 0.506847 0.345754 0.190747 0.068541 -0.0670649 -0.178073 -0.274401 -0.39724 -0.488853 -0.604972 -0.69781 -0.786221 -0.819447 -0.841549 -0.844402 -0.826691 -0.793864 -0.675469 -0.568668 -0.448639 -0.320104 -0.170175 -0.0429653 0.0305146 0.0793473 0.0699196 0.0640973 0.0805201 0.150082 0.181396 0.197088 0.198963 0.180105 0.171165 0.165355 0.153306 0.13932 0.121808 0.0960002 0.0636099 0.0420603 -0.00201266 0.00297022 -0.01075 0.00571671 -0.00988571 0.00495291 0.0103249 0.00202747 0.0288255 0.0069893 0.0178273 0.0250209 0.0224901 0.0358623 0.0699138 0.102137 0.10949 0.11849 0.10308 0.111049 0.0839795 0.0920491 0.0780812 0.0147155 -0.00169821 -0.0684824 -0.0785696 -0.109119 -0.146943 -0.162811 -0.181733 -0.209178 -0.219693 -0.240246 -0.290861 -0.325217 -0.342098 -0.33184 -0.326633 -0.326586 -0.31912 -0.277116 -0.277141 -0.223263 -0.236212 -0.206542 -0.19626 -0.228947 -0.189133 -0.223001 -0.149645 -0.0838313 -0.0133102 0.163725 0.256114 0.421735 0.631722 0.682114 0.729984 0.93726 1.09293 1.28009 1.49584 1.72019 1.86835 2.02145 2.21618 2.34229 2.46325 2.53789 2.65541 2.73194 2.85955 2.95762 2.96603 2.96058 2.95891 2.95569 2.91727 2.80246 2.68659 2.57868 2.47708 2.2992 2.01896 1.74869 1.51462 1.3187 1.18746 1.05576 0.908616 0.830028 0.739029 0.651653 0.530747 0.388129 0.324211 0.242146 0.168678 0.0863523 0.0236671 -0.0839184 -0.0800554 -0.146909 -0.167473 -0.150538 -0.158735 -0.152336 -0.16534 -0.185551 -0.158731 -0.140394 -0.142578 -0.116991 -0.0850929 -0.0697462 -0.0450458 -0.0364698 -0.000127659 0.0338857 0.078641 0.117068 0.148431 0.189871 0.199095 0.210383 0.198386 0.186583 0.179602 0.184848 0.218013 0.245189 0.283426 0.301817 0.307612 0.296696 0.299305 0.294508 0.280687 0.232099 0.180907 0.142642 0.132103 0.0890572 0.0929794 0.0802994 0.105767 0.0956161 0.107958 0.137663 0.17853 0.226585 0.25607 0.312746 0.349165 0.329886 0.34196 0.320422 0.315179 0.291192 0.3079 0.314621 0.302816 0.315746 0.299943 0.312783 0.295291 0.311099 0.307061 0.306201 0.305492 0.30171 0.295666 0.32583 0.395455 0.433459 0.439052 0.434099 0.416258 0.374538 0.288497 0.244122 0.186434 0.167451 0.114415 0.0844772 0.0492667 -0.0180291 -0.0500551 -0.0794268 -0.0812664 -0.077118 -0.095383 -0.0791651 -0.0769473 -0.0545597 -0.0568752 -0.0169918 0.00182556 0.0433963 0.0529215 0.0838224 0.100101 0.122122 0.140265 0.153132 0.141102 0.132322 0.137347 0.121121 0.0987212 0.0428967 0.0255213 0.0342949 0.0263121 0.0250735 0.00822722 0.0441839 0.0503725 0.0592927 0.0427369 -0.0235108 0.0775629 0.0610058 0.0746821 0.083853 0.09749 0.144272 0.248956 0.342323 0.420714 0.465772 0.416131 0.39786 0.46604 0.443219 0.387939 0.380844 0.338829 0.223713 0.219774 0.210273 0.109692 0.0731819 0.0304881 -0.020073 -0.0639773 0.0244142 0.04713 0.0781298 0.141583 0.180078 0.182555 0.220048 0.249642 0.228741 0.224981 0.212133 0.0998472 0.012332 -0.0335381 -0.0885489 -0.0822085 -0.0903375 -0.134731 -0.118117 -0.155019 -0.161754 -0.171878 -0.210084 -0.199128 -0.244047 -0.215387 -0.22573 -0.172014 -0.174843 -0.134947 -0.0960268 -0.0676995 -0.06087 -0.0692338 -0.0634511 -0.0423288 -0.0448088 -0.0446223 -0.0235764 -0.00735768 0.0146972 -0.00583278 -0.0223628 -0.0134332 0.0139785 0.0686271 0.0637796 0.0239944 0.0253558 -0.0208297 -0.0781398 -0.0555696 -0.0424126 -0.0301689 -0.0124231 -0.0241621 -0.0130691 -0.0464724 0.00253539 0.000584039 0.020526 -0.0416505 -0.073897 -0.0616008 -0.0203182 0.00332684 0.00528847 0.00517581 0.00566657 -0.00934216 -0.0358487 -0.0545588 -0.0245896 -0.0296515 0.0100897 0.00912027 -0.0253673 -0.0141384 -0.058436 -0.0396279 -0.0672999 -0.0104919 -0.02699 0.0106209 -0.014081 -0.0339897 -0.0311808 -0.017823 0.00978551 -0.000147978 0.00252752 -0.0548556 -0.0449829 -0.0410918 -0.015367 -0.013868 -0.00370243 -0.0154492 -0.0308341 -0.00720133 -0.0250824 -0.0154516 0.0109078 -0.00194524 0.0095693 0.032796 -0.00557828 0.0554963 0.0814005 0.0831926 0.0713162 0.0347101 -0.013725 -0.0419374 -0.0777558 -0.0899477 -0.116524 -0.121501 -0.186256 -0.232502 -0.221319 -0.240055 -0.259034 -0.320425 -0.330268 -0.334638 -0.36903 -0.292822 -0.223568 -0.282389 -0.222375 -0.126152 -0.0275805 0.0142455 0.0504481 0.126876 0.216316 0.286367 0.40209 0.557815 0.763816 0.91031 1.09256 1.29984 1.43569 1.59225 1.75795 1.84997 1.85379 1.91936 1.95691 2.02104 2.04485 2.09179 2.10283 2.13033 2.10077 2.10504 2.1573 2.16753 2.17772 2.15919 2.12874 2.09011 2.09893 2.21482 2.19974 2.1673 2.11618 1.98931 1.88941 1.82472 1.81914 1.81941 1.82869 1.79724 1.70312 1.62077 1.59092 1.5658 1.48065 1.40678 1.29332 1.31128 1.35197 1.37283 1.36046 1.28552 1.1754 1.08127 0.953009 0.823977 0.667224 0.509765 0.273756 0.187708 0.172625 0.208739 0.194792 0.207197 0.209587 0.197276 0.160901 0.114124 0.0569965 0.00651961 0.000288216 0.00863642 -0.0199998 -0.00873142 -0.00541012 -0.0053614 -0.00729697 -0.0039114 0.00389161 0.0296525 0.058983 0.050414 0.0610391 0.0378536 0.0131258 -0.0162506 -0.00793322 -0.00765706 0.011227 0.0111968 0.000259411 0.0055222 0.00890876 0.00510739 0.000406525 0.000212093 0.00700829 -0.015901 -0.0186259 -0.00909455 -0.03214 -0.0433109 -0.0295294 -0.0186268 -0.0340072 -0.0323863 -0.0177126 0.00374604 0.00753462 0.0301895 0.0495967 0.061496 0.0629836 0.0571943 0.0423189 0.0292877 -0.00549693 -0.0189098 -0.00854719 -0.00855899 -0.0229614 -0.00876005 -0.0218628 -0.0308243 -0.0348244 -0.0237642 -0.0123552 -0.0194747 -0.0337293 -0.0707221 -0.129799 -0.130684 -0.159011 -0.204158 -0.234628 -0.27131 -0.269292 -0.269051 -0.199549 -0.0999529 0.0177416 0.085616 0.193159 0.307241 0.374652 0.379402 0.356115 0.349878 0.351684 0.331887 0.254676 0.14651 0.0549605 -0.0163855 -0.00452584 -0.00505777 -0.0490434 -0.0467191 -0.0977862 -0.0837818 -0.112211 -0.103178 -0.105163 -0.114098 -0.100346 -0.113259 -0.105924 -0.125221 -0.0987403 -0.0983695 -0.040143 -0.0041598 0.0159603 -0.0101696 0.0405302 0.0732614 0.0653416 0.0242907 -0.0262052 -0.0653331 -0.0723518 -0.0781318 -0.107708 -0.0451012 -0.00873541 -0.00143351 0.0159662 0.0182477 0.0200139 0.0251201 0.0328139 0.0293421 0.0499584 0.0529014 0.068175 0.0705567 0.0744766 0.0749435 0.0818377 0.0725617 0.0685639 0.0650798 0.0705898 0.0818885 0.0770044 0.16415 0.246573 0.298276 0.339768 0.422038 0.524078 0.592112 0.740118 0.963361 1.21565 1.5508 1.74798 1.94301 2.06124 2.15012 2.27877 2.3817 2.46063 2.61034 2.66256 2.73756 2.83182 2.82419 2.87919 2.9259 2.94953 2.88872 2.86728 2.88605 2.89063 2.88864 2.90552 2.85872 2.81828 2.75385 2.63481 2.43912 2.22929 1.9717 1.72023 1.49637 1.33236 1.21127 1.13031 0.965871 0.79463 0.666193 0.450407 0.261003 0.119818 0.0484351 0.0307184 0.00452035 0.00128116 -0.0333824 -0.0106837 -0.0221997 -0.0301157 -0.0349315 -0.0177262 -0.0198552 -0.019922 -0.0312114 -0.0212343 -0.0199121 -0.00764607 -0.000763247 0.0433191 0.0573191 0.0667666 0.0407007 0.0540917 0.0628966 0.0514066 0.0464936 0.0611601 0.0979397 0.111466 0.11488 0.107117 0.102398 0.118615 0.112023 0.103792 0.104512 0.093895 0.0865138 0.0581677 0.05714 0.0483612 0.0218115 0.00565163 -0.00857385 -0.019552 -0.0290605 -0.0342576 -0.0805707 -0.11234 -0.108093 -0.119519 -0.169465 -0.190198 -0.190401 -0.221937 -0.274478 -0.284573 -0.293196 -0.293214 -0.305003 -0.285656 -0.286253 -0.284899 -0.281179 -0.263049 -0.23807 -0.201918 -0.162053 -0.135688 -0.0800635 -0.0464613 -0.000772612 0.0355691 0.0745027 0.0934654 0.112552 0.151657 0.169515 0.196595 0.194433 0.178334 0.167925 0.141453 0.165144 0.13214 0.16426 0.128651 0.167113 0.145561 0.152929 0.152345 0.162202 0.147074 0.158078 0.118963 0.114507 0.0658229 0.044372 0.0256157 -0.0280785 -0.060058 -0.160604 -0.237843 -0.2834 -0.291999 -0.267662 -0.274744 -0.294722 -0.24621 -0.193573 -0.129235 -0.132507 -0.0794017 -0.0642124 -0.00748393 0.0562474 0.0400439 0.0641075 0.100356 0.143809 0.166601 0.149234 0.15562 0.176686 0.244467 0.241965 0.253574 0.199474 0.133038 0.0657431 0.0765421 0.0875199 0.0853772 0.059298 0.067381 0.028124 0.00829962 -0.0216259 -0.0156489 -0.00312925 -0.0129824 -0.0206226 -0.0604484 -0.0672102 -0.0542429 -0.0433762 -0.030066 -0.0547244 -0.037905 -0.0398366 -0.0291759 -0.0158587 -0.0339327 -0.0237599 -0.0165871 -0.0185651 -0.00510748 0.00987032 0.0409543 0.0461912 0.0101392 -0.0259031 -0.0183498 -0.0272279 -0.0262647 -0.0212934 -0.0310855 -0.0369444 -0.0230647 -0.0029699 0.0112097 0.0192737 0.0157645 -0.000474816 0.00932786 0.00560821 0.0157079 -0.00198225 -0.00924207 0.0133613 0.0237176 0.020802 -0.000839996 -0.0103501 -0.013369 0.00189199 -0.00824734 0.005365 0.0607308 0.0908855 0.0751609 0.103204 0.11973 0.138284 0.149275 0.155577 0.127238 0.131521 0.128495 0.130788 0.144148 0.143593 0.149774 0.111029 0.0919684 0.0580575 0.0307885 -0.00665053 -0.0431775 -0.0941424 -0.15888 -0.191811 -0.242776 -0.25901 -0.242815 -0.287161 -0.260813 -0.287544 -0.287665 -0.294378 -0.276535 -0.240556 -0.226606 -0.215173 -0.242733 -0.144363 -0.175233 -0.116902 0.0333616 0.213786 0.371298 0.6075 0.862494 1.12054 1.40518 1.6802 1.92012 2.11851 2.24026 2.37676 2.53863 2.72039 2.88277 2.97478 3.07201 3.12829 3.19664 3.14443 3.11929 3.2161 3.19186 3.16742 3.16088 3.14003 3.12474 3.10362 3.05496 2.98107 2.88164 2.62739 2.3187 2.06283 1.86114 1.65035 1.54524 1.44108 1.25735 1.04867 0.944228 0.805729 0.63839 0.481928 0.32206 0.210384 0.16743 0.138739 0.0650049 0.000346316 -0.0193028 -0.0154398 -0.0393316 -0.0732672 -0.0650117 -0.0806752 -0.0493313 -0.0542918 -0.0367688 -0.00653603 -0.00396643 0.00880194 0.00607072 0.0282924 0.00650001 0.00722631 0.00206784 0.0284353 0.0233864 0.040613 0.0446161 0.0627124 0.0434055 0.0527782 0.0262733 0.0175509 -0.00944226 -0.0013618 0.0144979 -0.00069975 0.0208885 0.00789961 0.0144385 0.0308302 -0.0018697 0.0065041 -0.00907886 -0.0187332 -0.0225513 -0.0111741 -0.0379723 -0.0345139 -0.0545837 -0.0549708 -0.0618102 -0.0602599 -0.0609755 -0.0392975 0.00340524 0.0860115 0.152667 0.199615 0.221326 0.221716 0.21682 0.209763 0.224873 0.231653 0.245319 0.283233 0.316766 0.311336 0.267851 0.177155 0.051932 -0.0674404 -0.150248 -0.23375 -0.32899 -0.411959 -0.525201 -0.615278 -0.704078 -0.740484 -0.776207 -0.782667 -0.767096 -0.717973 -0.596249 -0.384642 -0.159588 0.0288646 0.198476 0.338193 0.359475 0.288218 0.241025 0.252227 0.261592 0.290341 0.281552];
yaw = yaw * pi / 180; % to radian
yaw_filtered = pf_filter(yaw, particles, noise, lambda);
plot(1:length(yaw),yaw,'b', 1:length(yaw),yaw_filtered,'r');
legend('yaw', 'yaw filtered')


